CN110779553A - Calibration method for magnetometer data - Google Patents

Calibration method for magnetometer data Download PDF

Info

Publication number
CN110779553A
CN110779553A CN201911224059.8A CN201911224059A CN110779553A CN 110779553 A CN110779553 A CN 110779553A CN 201911224059 A CN201911224059 A CN 201911224059A CN 110779553 A CN110779553 A CN 110779553A
Authority
CN
China
Prior art keywords
angular velocity
magnetometer data
measured
attitude information
theoretical
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN201911224059.8A
Other languages
Chinese (zh)
Inventor
蔡浩原
李文宽
崔松叶
赵晟霖
刘春秀
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Shenzhen Qianhai Weisheng Intelligent Technology Co ltd
Institute of Electronics of CAS
Original Assignee
Shenzhen Qianhai Weisheng Intelligent Technology Co ltd
Institute of Electronics of CAS
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Shenzhen Qianhai Weisheng Intelligent Technology Co ltd, Institute of Electronics of CAS filed Critical Shenzhen Qianhai Weisheng Intelligent Technology Co ltd
Priority to CN201911224059.8A priority Critical patent/CN110779553A/en
Publication of CN110779553A publication Critical patent/CN110779553A/en
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices

Landscapes

  • Engineering & Computer Science (AREA)
  • Manufacturing & Machinery (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Navigation (AREA)

Abstract

一种磁力计数据的校准方法，方法包括：获取旋转重力加速度、上一时刻的磁力计数据以及上一时刻的姿态信息；根据加速度计测得的量测加速度、旋转重力加速度和上一时刻的姿态信息计算角速度误差；利用角速度误差对陀螺仪测得的量测角速度进行PI校正，得到角速度校正值；根据角速度校正值、上一时刻的姿态信息以及上一时刻的磁力计数据计算理论磁力计数据；根据理论磁力计数据以及磁力计测得的量测磁力计数据构建扩展卡尔曼滤波器；利用扩展卡尔曼滤波器，根据量测加速度以及量测角速度计算磁力计数据校准值。

Figure 201911224059

A method for calibrating magnetometer data, comprising: acquiring rotational gravitational acceleration, magnetometer data at the previous moment, and attitude information at the previous moment; Calculate the angular velocity error from the attitude information; use the angular velocity error to perform PI correction on the measured angular velocity measured by the gyroscope to obtain the angular velocity correction value; calculate the theoretical magnetometer according to the angular velocity correction value, the attitude information of the previous moment and the magnetometer data of the previous moment According to the theoretical magnetometer data and the measured magnetometer data measured by the magnetometer, the extended Kalman filter is constructed; the extended Kalman filter is used to calculate the magnetometer data calibration value according to the measured acceleration and the measured angular velocity.

Figure 201911224059

Description

磁力计数据的校准方法Calibration method for magnetometer data

技术领域technical field

本公开涉及惯性导航领域，具体地，涉及一种磁力计数据的校准方法。The present disclosure relates to the field of inertial navigation, and in particular, to a method for calibrating magnetometer data.

背景技术Background technique

磁场在确定航向角和姿态信息方面具有重要作用，但是容易受到周围铁磁材料的干扰。受到干扰时，磁场矢量理想的球体分布发生偏移和变形，呈现椭球分布。球心偏移原点的情况为硬磁干扰，球体变形的情况为软磁干扰。磁场校准是为了计算硬磁干扰和软磁干扰的参数，并对磁场数据进行补偿。Magnetic fields play an important role in determining heading angle and attitude information, but are susceptible to interference from surrounding ferromagnetic materials. When disturbed, the ideal spherical distribution of the magnetic field vector is shifted and deformed, showing an ellipsoidal distribution. The case where the center of the sphere is offset from the origin is hard magnetic interference, and the case where the sphere is deformed is soft magnetic interference. Magnetic field calibration is to calculate the parameters of hard and soft magnetic interference and to compensate the magnetic field data.

相关技术中，椭球拟合算法无法实现实时校准，并且对采集的磁场数据要求极高，当采集的磁场数据集较小或分布不均匀时，容易导致校准结果恶化，此外还需要用户执行特定的复杂的手势，降低用户体验感。现有方法中，还通过陀螺仪数据预测磁力计数据，并使用扩展卡尔曼滤波器融合预测数据和测量数据，实现磁力计校准，但是由于陀螺仪传感器存在不同程度的漂移现象，所以预测数据会发生偏移，使得磁力计校准极不稳定。In the related art, the ellipsoid fitting algorithm cannot realize real-time calibration, and has extremely high requirements on the collected magnetic field data. When the collected magnetic field data set is small or unevenly distributed, the calibration results are likely to deteriorate. The complex gestures degrade the user experience. In the existing method, the magnetometer data is also predicted by the gyroscope data, and the predicted data and the measured data are fused by using the extended Kalman filter to realize the magnetometer calibration. An offset occurs, making the magnetometer calibration extremely unstable.

发明内容SUMMARY OF THE INVENTION

(一)要解决的技术问题(1) Technical problems to be solved

有鉴于此，本公开提供了一种磁力计数据的校准方法，以解决上述技术问题。In view of this, the present disclosure provides a method for calibrating magnetometer data to solve the above technical problems.

(二)技术方案(2) Technical solutions

本公开提供了一种磁力计数据的校准方法，包括：获取旋转重力加速度、上一时刻的磁力计数据以及上一时刻的姿态信息；根据加速度计测得的量测加速度、所述旋转重力加速度和上一时刻的姿态信息计算角速度误差；利用所述角速度误差对陀螺仪测得的量测角速度进行PI校正，得到角速度校正值；根据所述角速度校正值、上一时刻的姿态信息以及上一时刻的磁力计数据计算理论磁力计数据；根据所述理论磁力计数据以及磁力计测得的量测磁力计数据构建扩展卡尔曼滤波器；利用所述扩展卡尔曼滤波器，根据所述量测加速度以及量测角速度计算磁力计数据校准值。The present disclosure provides a method for calibrating magnetometer data, including: acquiring rotational gravitational acceleration, magnetometer data at a previous moment, and attitude information at a previous moment; Calculate the angular velocity error with the attitude information of the last moment; use the angular velocity error to perform PI correction on the measured angular velocity measured by the gyroscope to obtain an angular velocity correction value; According to the angular velocity correction value, the attitude information of the previous moment and the last Calculate the theoretical magnetometer data from the magnetometer data at the moment; construct an extended Kalman filter according to the theoretical magnetometer data and the measured magnetometer data measured by the magnetometer; Acceleration and measured angular velocity calculate the calibration value of magnetometer data.

可选地，所述根据加速度计测得的量测加速度、所述旋转重力加速度和上一时刻的姿态信息计算角速度误差，包括：根据所述旋转重力加速度和上一时刻的姿态信息计算理论加速度；根据所述理论加速度和量测加速度计算所述角速度误差。Optionally, calculating the angular velocity error according to the measured acceleration measured by the accelerometer, the rotational gravitational acceleration and the attitude information at the last moment includes: calculating the theoretical acceleration according to the rotational gravitational acceleration and the attitude information at the last moment. ; Calculate the angular velocity error according to the theoretical acceleration and the measured acceleration.

可选地，所述理论加速度和角速度误差为：Optionally, the theoretical acceleration and angular velocity errors are:

Figure BDA0002301020910000021
Figure BDA0002301020910000021

Figure BDA0002301020910000022
Figure BDA0002301020910000022

其中，

Figure BDA0002301020910000023
为理论加速度，ea为角速度误差，Rk-1为上一时刻的姿态信息，g为旋转重力加速度，ak为量测加速度。in,
Figure BDA0002301020910000023
is the theoretical acceleration, e a is the angular velocity error, R k-1 is the attitude information at the previous moment, g is the rotational gravitational acceleration, and a k is the measured acceleration.

可选地，所述角速度校正值为：Optionally, the angular velocity correction value is:

Figure BDA0002301020910000024
Figure BDA0002301020910000024

其中，

Figure BDA0002301020910000028
为角速度校正值，ωk为量测角速度，ea为角速度误差，Kp为PI校正中的比例控制参数，Ki为PI校正中的积分控制参数。in,
Figure BDA0002301020910000028
is the angular velocity correction value, ω k is the measured angular velocity, e a is the angular velocity error, K p is the proportional control parameter in the PI calibration, and K i is the integral control parameter in the PI calibration.

可选地，所述根据所述角速度校正值、上一时刻的姿态信息以及上一时刻的磁力计数据计算理论磁力计数据，包括：根据所述角速度校正值计算当前时刻的姿态信息；根据所述当前时刻的姿态信息、上一时刻的姿态信息以及上一时刻的磁力计数据计算所述理论磁力计数据。Optionally, the calculating the theoretical magnetometer data according to the angular velocity correction value, the attitude information at the last moment, and the magnetometer data at the last moment includes: calculating the attitude information at the current moment according to the angular velocity correction value; The theoretical magnetometer data is calculated from the attitude information at the current moment, the attitude information at the previous moment, and the magnetometer data at the last moment.

可选地，所述角速度校正值包括横滚角校正值、俯仰角校正值和航向角校正值，所述根据所述角速度校正值计算当前时刻的姿态信息，包括：根据所述横滚角校正值、俯仰角校正值和航向角校正值计算当前时刻的姿态信息。Optionally, the angular velocity correction value includes a roll angle correction value, a pitch angle correction value, and a heading angle correction value, and the calculating the attitude information at the current moment according to the angular velocity correction value includes: correcting according to the roll angle value, pitch angle correction value and heading angle correction value to calculate the attitude information at the current moment.

可选地，所述理论磁力计数据为：Optionally, the theoretical magnetometer data is:

Figure BDA0002301020910000026
Figure BDA0002301020910000026

其中，

Figure BDA0002301020910000029
为理论磁力计数据，Rk为当前时刻的姿态信息，Rk-1为上一时刻的姿态信息，Bk-1为上一时刻的磁力计数据。in,
Figure BDA0002301020910000029
is the theoretical magnetometer data, R k is the attitude information at the current moment, R k-1 is the attitude information at the last moment, and B k-1 is the magnetometer data at the last moment.

可选地，所述根据所述理论磁力计数据以及磁力计测得的量测磁力计数据构建扩展卡尔曼滤波器，包括：根据所述理论磁力计数据以及量测磁力计数据计算状态转移矩阵和量测矩阵；利用所述状态转移矩阵和量测矩阵构建扩展卡尔曼滤波器。Optionally, the constructing an extended Kalman filter according to the theoretical magnetometer data and the measured magnetometer data measured by the magnetometer includes: calculating a state transition matrix according to the theoretical magnetometer data and the measured magnetometer data and measurement matrix; using the state transition matrix and measurement matrix to construct an extended Kalman filter.

可选地，所述根据所述理论磁力计数据以及量测磁力计数据计算状态转移矩阵和量测矩阵，包括：建立软磁参数矩阵和硬磁参数矩阵；根据所述理论磁力计数据、软磁参数矩阵和硬磁参数矩阵生成当前时刻状态值；根据所述当前时刻状态值以及量测磁力计数据得到所述状态转移矩阵和量测矩阵。Optionally, calculating the state transition matrix and the measurement matrix according to the theoretical magnetometer data and the measurement magnetometer data includes: establishing a soft magnetic parameter matrix and a hard magnetic parameter matrix; The magnetic parameter matrix and the hard magnetic parameter matrix generate the state value at the current time; the state transition matrix and the measurement matrix are obtained according to the state value at the current time and the measurement magnetometer data.

可选地，所述利用所述扩展卡尔曼滤波器，根据所述量测加速度以及量测角速度计算磁力计数据校准值，包括：将上一时刻状态值、上一时刻状态值的方差估计值、所述量测加速度、量测角速度输入所述扩展卡尔曼滤波器，得到校准后的当前时刻状态值和当前时刻状态值的方差估计值，所述校准后的当前时刻状态值中包含所述磁力计数据校准值。Optionally, the use of the extended Kalman filter to calculate the magnetometer data calibration value according to the measured acceleration and the measured angular velocity includes: calculating the state value at the last moment and the variance estimation value of the state value at the last moment. , the measured acceleration and the measured angular velocity are input to the extended Kalman filter to obtain the calibrated current state value and the variance estimated value of the current state value, and the calibrated current state value includes the Magnetometer data calibration value.

(三)有益效果(3) Beneficial effects

本公开提供的磁力计数据的校准方法，具有以下有益效果：The method for calibrating magnetometer data provided by the present disclosure has the following beneficial effects:

(1)通过利用六轴算法融合加速度计数据和陀螺仪数据，以准确地预测磁力计数据；(1) Accurately predict magnetometer data by fusing accelerometer data and gyroscope data using a six-axis algorithm;

(2)利用扩展卡尔曼滤波器实现磁力计校准，使得校准结果更为准确、稳定；(2) Using extended Kalman filter to achieve magnetometer calibration, making the calibration result more accurate and stable;

(3)无需使用者进行特定行为便可实现实时校准，校准更为便捷、快速，提升用户体验感。(3) Real-time calibration can be achieved without the need for users to perform specific behaviors, and the calibration is more convenient and fast, and the user experience is improved.

附图说明Description of drawings

图1示意性示出了本公开实施例提供的磁力计数据的校准方法的流程图；以及FIG. 1 schematically shows a flowchart of a method for calibrating magnetometer data provided by an embodiment of the present disclosure; and

图2示意性示出了本公开实施例提供的磁力计数据的校准方法进行磁力计校准后的效果示意图。FIG. 2 schematically shows a schematic diagram of the effect of the magnetometer data calibration method provided by the embodiment of the present disclosure after the magnetometer calibration is performed.

具体实施方式Detailed ways

为使本公开的目的、技术方案和优点更加清楚明白，以下结合具体实施例，并参照附图，对本公开进一步详细说明。In order to make the objectives, technical solutions and advantages of the present disclosure clearer, the present disclosure will be further described in detail below with reference to the specific embodiments and the accompanying drawings.

图1示意性示出了本公开实施例提供的磁力计数据的校准方法的流程图。FIG. 1 schematically shows a flowchart of a method for calibrating magnetometer data provided by an embodiment of the present disclosure.

参阅图1，同时结合图2，对图1所示方法进行详细说明。如图1所示，该磁力计数据的校准方法包括操作S110-操作S160。Referring to FIG. 1 and in conjunction with FIG. 2 , the method shown in FIG. 1 will be described in detail. As shown in FIG. 1 , the method for calibrating magnetometer data includes operations S110-operation S160.

S110，获取旋转重力加速度、上一时刻的磁力计数据以及上一时刻的姿态信息。S110: Acquire the rotational gravitational acceleration, the magnetometer data at the last moment, and the attitude information at the last moment.

本公开实施例中，例如可以实时监听微机电系统的九轴传感器，以获得磁力计、加速度计和陀螺仪在各时刻测得的数据，同时，还可以保存上一时刻的磁力计数据Bk-1和上一时刻的姿态信息Rk-1。In the embodiment of the present disclosure, for example, the nine-axis sensor of the micro-electromechanical system can be monitored in real time to obtain the data measured by the magnetometer, the accelerometer and the gyroscope at each moment, and at the same time, the magnetometer data B k of the previous moment can also be saved -1 and the attitude information R k-1 of the previous moment.

旋转重力加速度g表示在载体坐标系下重力加速度的值。载体坐标系是指以传感器的质心为原点，以传感器的三个敏感轴为坐标轴的坐标系。The rotational gravitational acceleration g represents the value of the gravitational acceleration in the carrier coordinate system. The carrier coordinate system refers to the coordinate system with the sensor's center of mass as the origin and the three sensitive axes of the sensor as the coordinate axes.

S120，根据加速度计测得的量测加速度、旋转重力加速度和上一时刻的姿态信息计算角速度误差。S120: Calculate the angular velocity error according to the measured acceleration, the rotational gravitational acceleration and the attitude information at the last moment measured by the accelerometer.

根据本公开的实施例，操作S120包括操作S120A和操作S120B。According to an embodiment of the present disclosure, operation S120 includes operation S120A and operation S120B.

在操作S120A中，根据旋转重力加速度和上一时刻的姿态信息计算理论加速度。具体地，利用旋转重力加速度g和上一时刻的姿态信息Rk-1，预测当前地点的重力加速度在载体坐标系中的值，从而获得理论加速度

Figure BDA0002301020910000041
理论加速度
Figure BDA0002301020910000042
为：In operation S120A, the theoretical acceleration is calculated according to the rotational gravitational acceleration and the attitude information at the last moment. Specifically, using the rotational gravitational acceleration g and the attitude information R k-1 at the last moment, predict the value of the gravitational acceleration at the current location in the carrier coordinate system, so as to obtain the theoretical acceleration
Figure BDA0002301020910000041
Theoretical acceleration
Figure BDA0002301020910000042
for:

Figure BDA0002301020910000043
Figure BDA0002301020910000043

其中，

Figure BDA0002301020910000044
为理论加速度，Rk-1为上一时刻的姿态信息，g为旋转重力加速度。in,
Figure BDA0002301020910000044
is the theoretical acceleration, R k-1 is the attitude information at the previous moment, and g is the rotational gravitational acceleration.

在操作S120B中，根据理论加速度和量测加速度计算角速度误差。具体地，将理论加速度

Figure BDA0002301020910000045
与加速度计测得的量测加速度ak叉乘，从而获得角速度误差ea，该角速度误差ea为：In operation S120B, an angular velocity error is calculated according to the theoretical acceleration and the measured acceleration. Specifically, the theoretical acceleration
Figure BDA0002301020910000045
It is cross-multiplied with the measured acceleration a k measured by the accelerometer to obtain the angular velocity error ea , and the angular velocity error ea is:

Figure BDA0002301020910000046
Figure BDA0002301020910000046

其中，

Figure BDA0002301020910000047
为理论加速度，ak为量测加速度。in,
Figure BDA0002301020910000047
is the theoretical acceleration, and a k is the measured acceleration.

S130，利用角速度误差对陀螺仪测得的量测角速度进行PI校正，得到角速度校正值。S130, using the angular velocity error to perform PI correction on the measured angular velocity measured by the gyroscope to obtain an angular velocity correction value.

本公开实施例中，通过比例积分(Proportional Integral，PI)控制器消除陀螺仪的漂移误差，只要存在误差，PI控制器便会持续作用，直至误差为0。In the embodiment of the present disclosure, the drift error of the gyroscope is eliminated by a proportional integral (PI) controller. As long as there is an error, the PI controller will continue to function until the error becomes zero.

根据本公开的实施例，得到的角速度校正值为：According to the embodiment of the present disclosure, the obtained angular velocity correction value is:

其中，为角速度校正值，ωk为量测角速度，ea为角速度误差，Kp为PI校正中的比例控制参数，Ki为PI校正中的积分控制参数。可以理解的是，ωk为k时刻的量测角速度，

Figure BDA0002301020910000058
为消除误差之后k时刻的角速度校正值。in, is the angular velocity correction value, ω k is the measured angular velocity, e a is the angular velocity error, K p is the proportional control parameter in the PI calibration, and K i is the integral control parameter in the PI calibration. It can be understood that ω k is the measured angular velocity at time k,
Figure BDA0002301020910000058
It is the angular velocity correction value at time k after the error is eliminated.

S140，根据角速度校正值、上一时刻的姿态信息以及上一时刻的磁力计数据计算理论磁力计数据。S140: Calculate theoretical magnetometer data according to the angular velocity correction value, the attitude information at the last moment, and the magnetometer data at the last moment.

本公开实施例中，传感器的姿态信息主要包括旋转矩阵R、四元数q和角速度，角速度包括横滚角

Figure BDA0002301020910000054
俯仰角θ和航向角φ，他们之间的转换关系如下：In the embodiment of the present disclosure, the attitude information of the sensor mainly includes the rotation matrix R, the quaternion q and the angular velocity, and the angular velocity includes the roll angle
Figure BDA0002301020910000054
The pitch angle θ and the heading angle φ, the conversion relationship between them is as follows:

q＝w+xi+yj+zkq=w+xi+yj+zk

Figure BDA0002301020910000055
Figure BDA0002301020910000055

Figure BDA0002301020910000056
Figure BDA0002301020910000056

根据本公开的实施例，操作S140包括操作S140A和操作S140B。According to an embodiment of the present disclosure, operation S140 includes operation S140A and operation S140B.

在操作S140A中，根据角速度校正值计算当前时刻的姿态信息。根据上述转换关系，利用角速度校正值

Figure BDA0002301020910000059
对四元数进行更新，得到实时的姿态信息。根据本公开的实施例，角速度校正值
Figure BDA00023010209100000510
包括横滚角校正值、俯仰角校正值和航向角校正值，可以根据横滚角校正值、俯仰角校正值和航向角校正值计算当前时刻的姿态信息Rk。In operation S140A, attitude information at the current moment is calculated according to the angular velocity correction value. According to the above conversion relationship, use the angular velocity correction value
Figure BDA0002301020910000059
Update the quaternion to get real-time attitude information. According to an embodiment of the present disclosure, the angular velocity correction value
Figure BDA00023010209100000510
The roll angle correction value, the pitch angle correction value and the heading angle correction value are included, and the attitude information R k at the current moment can be calculated according to the roll angle correction value, the pitch angle correction value and the heading angle correction value.

本公开实施例中，由于加速度计在计算俯仰角和横滚角时具有长期稳定性，所以该磁力计数据的校准方法得到的姿态信息，可以避免俯仰角和横滚角方向上的漂移，极大提高了姿态的稳定性。In the embodiment of the present disclosure, since the accelerometer has long-term stability when calculating the pitch angle and the roll angle, the attitude information obtained by the method for calibrating the magnetometer data can avoid the drift in the pitch angle and the roll angle direction. Greatly improved the stability of the posture.

在操作S140B中，根据当前时刻的姿态信息、上一时刻的姿态信息以及上一时刻的磁力计数据计算理论磁力计数据。In operation S140B, theoretical magnetometer data is calculated according to the attitude information at the current moment, the attitude information at the last moment, and the magnetometer data at the last moment.

本公开实施例中，通过当前时刻的姿态信息Rk和上一时刻的姿态信息Rk-1，计算得到姿态信息的改变值ΔRk，然后使用姿态信息的改变值ΔRk对上一时刻的磁力计数据Bk-1进行预测，即可得到当前时刻磁力计数据的理论值，即得到当前时刻的理论磁力计数据

Figure BDA0002301020910000061
In the embodiment of the present disclosure, the attitude information R k at the current moment and the attitude information R k-1 at the previous moment are used to calculate the change value ΔR k of the attitude information, and then the change value ΔR k of the attitude information is used to calculate the change value ΔR k of the attitude information at the previous moment. The magnetometer data B k-1 is predicted, and the theoretical value of the magnetometer data at the current moment can be obtained, that is, the theoretical magnetometer data at the current moment can be obtained.
Figure BDA0002301020910000061

根据本公开的实施例，理论磁力计数据为：According to an embodiment of the present disclosure, the theoretical magnetometer data is:

其中，

Figure BDA0002301020910000063
为理论磁力计数据，Rk为当前时刻的姿态信息，Rk-1为上一时刻的姿态信息，Bk-1为上一时刻的磁力计数据。in,
Figure BDA0002301020910000063
is the theoretical magnetometer data, R k is the attitude information at the current moment, R k-1 is the attitude information at the last moment, and B k-1 is the magnetometer data at the last moment.

S150，根据理论磁力计数据以及磁力计测得的量测磁力计数据构建扩展卡尔曼滤波器。S150 , constructing an extended Kalman filter according to the theoretical magnetometer data and the measured magnetometer data measured by the magnetometer.

根据本公开的实施例，操作S150包括操作S150A和操作S150B。According to an embodiment of the present disclosure, operation S150 includes operation S150A and operation S150B.

在操作S150A中，根据理论磁力计数据以及量测磁力计数据计算状态转移矩阵和量测矩阵。具体地，根据本公开的实施例，建立软磁参数矩阵W和硬磁参数矩阵V，根据理论磁力计数据

Figure BDA0002301020910000064
软磁参数矩阵W和硬磁参数矩阵V生成当前时刻状态值Xk，根据当前时刻状态值Xk以及量测磁力计数据Bk得到状态转移矩阵和量测矩阵。In operation S150A, a state transition matrix and a measurement matrix are calculated according to the theoretical magnetometer data and the measurement magnetometer data. Specifically, according to an embodiment of the present disclosure, a soft magnetic parameter matrix W and a hard magnetic parameter matrix V are established, according to theoretical magnetometer data
Figure BDA0002301020910000064
The soft magnetic parameter matrix W and the hard magnetic parameter matrix V generate the current state value X k , and the state transition matrix and the measurement matrix are obtained according to the current state value X k and the measured magnetometer data B k .

本公开实施例中，生成的当前时刻状态值为Xk：In this embodiment of the present disclosure, the generated state value at the current moment is X k :

Figure BDA0002301020910000065
Figure BDA0002301020910000065

其中，W11、W22、W33、W12、W13、W13为软磁参数矩阵W中相应位置的元素。Wherein, W 11 , W 22 , W 33 , W 12 , W 13 , and W 13 are elements at corresponding positions in the soft magnetic parameter matrix W.

进一步地，根据当前时刻状态值Xk和量测磁力计数据Bk中各变量之间的关系，计算相应的状态转移矩阵Fk和量测矩阵Hk。Further, according to the relationship between the current state value X k and the variables in the measured magnetometer data B k , the corresponding state transition matrix F k and the measurement matrix H k are calculated.

具体地，根据以下关系式计算状态转移矩阵Fk：Specifically, the state transition matrix F k is calculated according to the following relation:

Xk+1＝FkXk+dk X k+1 =F k X k +d k

Figure BDA0002301020910000071
Figure BDA0002301020910000071

Figure BDA0002301020910000072
Figure BDA0002301020910000072

其中，dk为状态转移过程中的噪声，Δk为k+1时刻和k时刻之间的时间差。Among them, d k is the noise in the state transition process, and Δk is the time difference between time k+1 and time k.

具体地，根据以下关系式计算量测矩阵Hk：Specifically, the measurement matrix H k is calculated according to the following relation:

Zk＝HkXk+vk Z k =H k X k +v k

其中，Zk为k时刻磁力计的量测值，vk为k时刻磁力计量测值的量测误差。Among them, Z k is the measured value of the magnetometer at time k, and v k is the measurement error of the measured value of the magnetometer at time k.

在操作S150B中，利用状态转移矩阵和量测矩阵构建扩展卡尔曼滤波器。本公开实施例中，扩展卡尔曼滤波器中的计算参数包括状态转移矩阵Fk和量测矩阵Hk。In operation S150B, an extended Kalman filter is constructed using the state transition matrix and the measurement matrix. In the embodiment of the present disclosure, the calculation parameters in the extended Kalman filter include a state transition matrix F k and a measurement matrix H k .

S160，利用扩展卡尔曼滤波器，根据量测加速度以及量测角速度计算磁力计数据校准值。S160, using the extended Kalman filter to calculate the magnetometer data calibration value according to the measured acceleration and the measured angular velocity.

根据本公开的实施例，将上一时刻状态值Xk-1、上一时刻状态值的方差估计值Pk-1、量测加速度ak、量测角速度ωk输入扩展卡尔曼滤波器，得到校准后的当前时刻状态值Xk和当前时刻状态值的方差估计值Pk，校准后的当前时刻状态值Xk中包含磁力计数据校准值。According to the embodiment of the present disclosure, the state value X k-1 at the last moment, the variance estimation value P k-1 of the state value at the last moment, the measured acceleration ak , and the measured angular velocity ω k are input into the extended Kalman filter, The calibrated current state value X k and the variance estimation value P k of the current state value are obtained, and the calibrated current state value X k includes the magnetometer data calibration value.

具体地，根据扩展卡尔曼滤波器，进行磁力计数据校准的公式为：Specifically, according to the extended Kalman filter, the formula for calibrating the magnetometer data is:

预测阶段：Prediction stage:

Xk|k-1＝Fk-1Xk-1 X k|k-1 =F k-1 X k-1

Figure BDA0002301020910000073
Figure BDA0002301020910000073

更新阶段：Update phase:

Xk＝Xk|k-1+Kk(Zk-HkXk|k-1)X k =X k|k-1 +K k (Z k -H k X k|k-1 )

Pk＝(1-KkHk)Pk|k-1 P k =(1-K k H k )P k|k-1

其中，Pl(l＝0，1，2，…，k)是通过扩展卡尔曼滤波器计算得到的l时刻状态值的方差估计值，其初始值P0可以设为单位阵，之后每次进行扩展卡尔曼滤波时都会输出新的方差估计值，用于下一次扩展卡尔曼滤波。Fk-1是k-1时刻的状态转移矩阵，Xk-1是k-1时刻的状态值，Xk|k-1是状态值从k-1时刻到k时刻的一步预测值，Fk-1是k-1时刻的状态转移矩阵，Pk-1是k-1时刻的状态值的方差估计值，Qk-1是状态转移过程中k-1时刻的噪声的方差矩阵，Kk是滤波增益矩阵，是使估计的均方差达到最小的最佳增益矩阵，Hk是k时刻的量测矩阵，Rk是k时刻测量误差的协方差矩阵，Pk|k-1是状态值的方差估计值从k-1时刻到k时刻的一步预测值，Zk是k时刻磁力计的量测值，Xk、Pk是计算求得的当前时刻(即k时刻)的状态值和方差估计值。Xk中包含磁力计数据校准值。Among them, P l (l=0, 1, 2, ..., k) is the estimated variance of the state value at time l calculated by the extended Kalman filter, and its initial value P 0 can be set as a unit matrix. When performing extended Kalman filtering, a new variance estimate is output for the next extended Kalman filtering. F k-1 is the state transition matrix at time k-1, X k-1 is the state value at time k-1, X k|k-1 is the one-step prediction value of the state value from time k-1 to time k, F k-1 is the state transition matrix at time k-1, P k-1 is the estimated variance of the state value at time k-1, Q k-1 is the variance matrix of noise at time k-1 in the state transition process, K k is the filter gain matrix, which is the best gain matrix to minimize the estimated mean square error, H k is the measurement matrix at time k, R k is the covariance matrix of the measurement error at time k, and P k|k-1 is the state The estimated variance of the value is a one-step prediction value from time k-1 to time k, Z k is the measured value of the magnetometer at time k, X k , P k are the calculated state values at the current time (that is, time k) and variance estimates. Magnetometer data calibration values are included in X k .

图2示意性示出了本公开实施例提供的磁力计数据的校准方法进行磁力计校准后的效果示意图。参阅图2，可以看出。本公开实施例中的磁力计数据的校准方法能够抑制校准后磁力计数据的漂移，实现更加稳定的磁场校准。FIG. 2 schematically shows a schematic diagram of the effect of the magnetometer data calibration method provided by the embodiment of the present disclosure after the magnetometer calibration is performed. Referring to Figure 2, it can be seen. The method for calibrating magnetometer data in the embodiment of the present disclosure can suppress the drift of magnetometer data after calibration, and realize a more stable magnetic field calibration.

综上所述，本公开实施例中的磁力计数据的校准方法，利用六轴融合算法融合加速度计数据和陀螺仪数据，使用加速度修正陀螺仪数据，并使用修正后的陀螺仪数据对磁力计进行旋转，以更准确地预测磁力计数据，然后使用扩展卡尔曼滤波融合预测值和磁力计量测值，实现磁力计的动态校准，可以抑制磁力计数据的漂移，实现了稳定的、高精度的实时磁力计校准。该磁力计数据的校准方法在消费电子、车辆惯性导航系统、手机、AR眼镜等需要用到磁力计来确定姿态的设备中具有广泛的应用潜力。To sum up, the method for calibrating magnetometer data in this embodiment of the present disclosure uses a six-axis fusion algorithm to fuse accelerometer data and gyroscope data, uses acceleration to correct gyroscope data, and uses the corrected gyroscope data to calibrate the magnetometer data. Rotate to predict magnetometer data more accurately, and then use extended Kalman filter to fuse the predicted value and magnetometer measurement value to achieve dynamic calibration of the magnetometer, which can suppress the drift of magnetometer data and achieve stable and high precision. Real-time magnetometer calibration. The method for calibrating magnetometer data has broad application potential in consumer electronics, vehicle inertial navigation systems, mobile phones, AR glasses, and other devices that require a magnetometer to determine attitude.

以上所述的具体实施例，对本公开的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上所述仅为本公开的具体实施例而已，并不用于限制本公开，凡在本公开的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本公开的保护范围之内。The specific embodiments described above further describe the purpose, technical solutions and beneficial effects of the present disclosure in detail. It should be understood that the above-mentioned specific embodiments are only specific embodiments of the present disclosure, and are not intended to limit the present disclosure. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present disclosure should be included within the protection scope of the present disclosure.

Claims (10)

1.一种磁力计数据的校准方法，包括：1. A method for calibrating magnetometer data, comprising: 获取旋转重力加速度、上一时刻的磁力计数据以及上一时刻的姿态信息；Obtain the rotational gravitational acceleration, the magnetometer data at the last moment, and the attitude information at the last moment; 根据加速度计测得的量测加速度、所述旋转重力加速度和上一时刻的姿态信息计算角速度误差；Calculate the angular velocity error according to the measured acceleration measured by the accelerometer, the rotational gravitational acceleration and the attitude information at the previous moment; 利用所述角速度误差对陀螺仪测得的量测角速度进行PI校正，得到角速度校正值；Use the angular velocity error to perform PI correction on the measured angular velocity measured by the gyroscope to obtain an angular velocity correction value; 根据所述角速度校正值、上一时刻的姿态信息以及上一时刻的磁力计数据计算理论磁力计数据；Calculate theoretical magnetometer data according to the angular velocity correction value, the attitude information at the last moment, and the magnetometer data at the last moment; 根据所述理论磁力计数据以及磁力计测得的量测磁力计数据构建扩展卡尔曼滤波器；constructing an extended Kalman filter according to the theoretical magnetometer data and the measured magnetometer data measured by the magnetometer; 利用所述扩展卡尔曼滤波器，根据所述量测加速度以及量测角速度计算磁力计数据校准值。Using the extended Kalman filter, a magnetometer data calibration value is calculated from the measured acceleration and measured angular velocity. 2.根据权利要求1所述的方法，其中，所述根据加速度计测得的量测加速度、所述旋转重力加速度和上一时刻的姿态信息计算角速度误差，包括：2. The method according to claim 1, wherein the calculating the angular velocity error according to the measured acceleration measured by the accelerometer, the rotational gravitational acceleration and the attitude information of the previous moment, comprising: 根据所述旋转重力加速度和上一时刻的姿态信息计算理论加速度；Calculate the theoretical acceleration according to the rotational gravitational acceleration and the attitude information at the previous moment; 根据所述理论加速度和量测加速度计算所述角速度误差。The angular velocity error is calculated from the theoretical acceleration and the measured acceleration. 3.根据权利要求2所述的方法，其中，所述理论加速度和角速度误差为：3. The method according to claim 2, wherein the theoretical acceleration and angular velocity errors are:
Figure FDA0002301020900000011
Figure FDA0002301020900000011
 其中，
Figure FDA0002301020900000013
为理论加速度，ea为角速度误差，Rk-1为上一时刻的姿态信息，g为旋转重力加速度，ak为量测加速度。in,
Figure FDA0002301020900000013
is the theoretical acceleration, e a is the angular velocity error, R k-1 is the attitude information at the previous moment, g is the rotational gravitational acceleration, and a k is the measured acceleration. 4.根据权利要求1所述的方法，其中，所述角速度校正值为：4. The method of claim 1, wherein the angular velocity correction value is:
Figure FDA0002301020900000014
Figure FDA0002301020900000014
 其中，
Figure FDA0002301020900000015
为角速度校正值，ωk为量测角速度，ea为角速度误差，Kp为PI校正中的比例控制参数，Ki为PI校正中的积分控制参数。in,
Figure FDA0002301020900000015
is the angular velocity correction value, ω k is the measured angular velocity, e a is the angular velocity error, K p is the proportional control parameter in the PI calibration, and K i is the integral control parameter in the PI calibration. 5.根据权利要求1所述的方法，其中，所述根据所述角速度校正值、上一时刻的姿态信息以及上一时刻的磁力计数据计算理论磁力计数据，包括：5. The method according to claim 1, wherein the calculating the theoretical magnetometer data according to the angular velocity correction value, the attitude information at the last moment and the magnetometer data at the last moment comprises: 根据所述角速度校正值计算当前时刻的姿态信息；Calculate the attitude information at the current moment according to the angular velocity correction value; 根据所述当前时刻的姿态信息、上一时刻的姿态信息以及上一时刻的磁力计数据计算所述理论磁力计数据。The theoretical magnetometer data is calculated according to the attitude information at the current moment, the attitude information at the last moment, and the magnetometer data at the last moment. 6.根据权利要求5所述的方法，其中，所述角速度校正值包括横滚角校正值、俯仰角校正值和航向角校正值，所述根据所述角速度校正值计算当前时刻的姿态信息，包括：6. The method according to claim 5, wherein the angular velocity correction value comprises a roll angle correction value, a pitch angle correction value and a heading angle correction value, and the attitude information at the current moment is calculated according to the angular velocity correction value, include: 根据所述横滚角校正值、俯仰角校正值和航向角校正值计算当前时刻的姿态信息。The attitude information at the current moment is calculated according to the roll angle correction value, the pitch angle correction value and the heading angle correction value. 7.根据权利要求5所述的方法，其中，所述理论磁力计数据为：7. The method of claim 5, wherein the theoretical magnetometer data is:
Figure FDA0002301020900000021
Figure FDA0002301020900000021
 其中，
Figure FDA0002301020900000022
为理论磁力计数据，Rk为当前时刻的姿态信息，Rk-1为上一时刻的姿态信息，Bk-1为上一时刻的磁力计数据。in,
Figure FDA0002301020900000022
is the theoretical magnetometer data, R k is the attitude information at the current moment, R k-1 is the attitude information at the last moment, and B k-1 is the magnetometer data at the last moment. 8.根据权利要求1所述的方法，其中，所述根据所述理论磁力计数据以及磁力计测得的量测磁力计数据构建扩展卡尔曼滤波器，包括：8. The method according to claim 1, wherein the constructing an extended Kalman filter according to the theoretical magnetometer data and the measured magnetometer data measured by the magnetometer comprises: 根据所述理论磁力计数据以及量测磁力计数据计算状态转移矩阵和量测矩阵；Calculate the state transition matrix and the measurement matrix according to the theoretical magnetometer data and the measurement magnetometer data; 利用所述状态转移矩阵和量测矩阵构建扩展卡尔曼滤波器。An extended Kalman filter is constructed using the state transition matrix and measurement matrix. 9.根据权利要求8所述的方法，其中，所述根据所述理论磁力计数据以及量测磁力计数据计算状态转移矩阵和量测矩阵，包括：9. The method according to claim 8, wherein the calculating a state transition matrix and a measurement matrix according to the theoretical magnetometer data and the measurement magnetometer data comprises: 建立软磁参数矩阵和硬磁参数矩阵；Establish soft magnetic parameter matrix and hard magnetic parameter matrix; 根据所述理论磁力计数据、软磁参数矩阵和硬磁参数矩阵生成当前时刻状态值；Generate the current state value according to the theoretical magnetometer data, the soft magnetic parameter matrix and the hard magnetic parameter matrix; 根据所述当前时刻状态值以及量测磁力计数据得到所述状态转移矩阵和量测矩阵。The state transition matrix and the measurement matrix are obtained according to the current state value and the measurement magnetometer data. 10.根据权利要求9所述的方法，其中，所述利用所述扩展卡尔曼滤波器，根据所述量测加速度以及量测角速度计算磁力计数据校准值，包括：10. The method according to claim 9, wherein the calculating a magnetometer data calibration value according to the measured acceleration and the measured angular velocity using the extended Kalman filter comprises: 将上一时刻状态值、上一时刻状态值的方差估计值、所述量测加速度、量测角速度输入所述扩展卡尔曼滤波器，得到校准后的当前时刻状态值和当前时刻状态值的方差估计值，所述校准后的当前时刻状态值中包含所述磁力计数据校准值。Input the state value at the last moment, the estimated variance of the state value at the last moment, the measured acceleration, and the measured angular velocity into the extended Kalman filter to obtain the calibrated state value at the current moment and the variance of the state value at the current moment The estimated value, the calibrated current state value includes the magnetometer data calibration value.
CN201911224059.8A 2019-12-03 2019-12-03 Calibration method for magnetometer data Pending CN110779553A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201911224059.8A CN110779553A (en) 2019-12-03 2019-12-03 Calibration method for magnetometer data

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201911224059.8A CN110779553A (en) 2019-12-03 2019-12-03 Calibration method for magnetometer data

Publications (1)

Publication Number Publication Date
CN110779553A true CN110779553A (en) 2020-02-11

Family

ID=69393694

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201911224059.8A Pending CN110779553A (en) 2019-12-03 2019-12-03 Calibration method for magnetometer data

Country Status (1)

Country Link
CN (1) CN110779553A (en)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114279426A (en) * 2021-12-30 2022-04-05 杭州电子科技大学 A six-axis optimized magnetometer online calibration method
CN114353825A (en) * 2021-12-06 2022-04-15 重庆邮电大学 On-line calibration algorithm, medium and system of magnetometer based on unscented Kalman filter
CN115900770A (en) * 2023-02-14 2023-04-04 北京理工大学前沿技术研究院 Online correction method and system for magnetic sensor in airborne environment

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN202974288U (en) * 2012-12-05 2013-06-05 南京理工大学 Miniature strapdown navigation attitude system
CN106095113A (en) * 2016-06-27 2016-11-09 南京睿悦信息技术有限公司 The measuring and calculating of user's attitude and the virtual reality follow-up method that a kind of nine axle sensors merge
CN106293103A (en) * 2016-10-21 2017-01-04 北京工商大学 Four-axle aircraft gesture control device based on inertial sensor and control method
CN106996780A (en) * 2017-04-24 2017-08-01 湖南格纳微信息科技有限公司 A kind of course error modification method and device and magnetic field detection method and device
US20180110569A1 (en) * 2016-10-26 2018-04-26 Prichard Medical, LLC Surgical instrument with led lighting and absolute orientation
EP3350543A1 (en) * 2015-09-16 2018-07-25 Raytheon Company Magnetic field gradient navigation aid
CN108534744A (en) * 2018-01-30 2018-09-14 歌尔科技有限公司 A kind of attitude angle acquisition methods, device and handle

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN202974288U (en) * 2012-12-05 2013-06-05 南京理工大学 Miniature strapdown navigation attitude system
EP3350543A1 (en) * 2015-09-16 2018-07-25 Raytheon Company Magnetic field gradient navigation aid
CN106095113A (en) * 2016-06-27 2016-11-09 南京睿悦信息技术有限公司 The measuring and calculating of user's attitude and the virtual reality follow-up method that a kind of nine axle sensors merge
CN106293103A (en) * 2016-10-21 2017-01-04 北京工商大学 Four-axle aircraft gesture control device based on inertial sensor and control method
US20180110569A1 (en) * 2016-10-26 2018-04-26 Prichard Medical, LLC Surgical instrument with led lighting and absolute orientation
CN106996780A (en) * 2017-04-24 2017-08-01 湖南格纳微信息科技有限公司 A kind of course error modification method and device and magnetic field detection method and device
CN108534744A (en) * 2018-01-30 2018-09-14 歌尔科技有限公司 A kind of attitude angle acquisition methods, device and handle

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
KE HAN等: "Extended Kalman Filter-Based Gyroscope-Aided", 《IEEE SENSORS JOURNAL》 *
朱建良等: "基于椭球曲面拟合的三维磁罗盘误差补偿算法", 《中国惯性技术学报》 *
范冰飞: "人体运动姿态传感器抗磁场和大加速度干扰的方法及其步态分析应用研究", 《中国博士学位论文全文数据库》 *

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114353825A (en) * 2021-12-06 2022-04-15 重庆邮电大学 On-line calibration algorithm, medium and system of magnetometer based on unscented Kalman filter
CN114353825B (en) * 2021-12-06 2023-11-03 重庆邮电大学 Magnetometer online calibration algorithm, medium and system based on unscented Kalman filtering
CN114279426A (en) * 2021-12-30 2022-04-05 杭州电子科技大学 A six-axis optimized magnetometer online calibration method
CN114279426B (en) * 2021-12-30 2023-12-15 杭州电子科技大学 Six-axis optimized magnetometer online calibration method
CN115900770A (en) * 2023-02-14 2023-04-04 北京理工大学前沿技术研究院 Online correction method and system for magnetic sensor in airborne environment

Similar Documents

Publication Publication Date Title
CN106647791B (en) Three-dimensional attitude measurement and control device, mechanical equipment and three-dimensional attitude measurement and control method
CN110887481B (en) Carrier dynamic attitude estimation method based on MEMS inertial sensor
CN109916395B (en) Gesture autonomous redundant combined navigation algorithm
CN107014376B (en) A kind of posture inclination angle estimation method suitable for the accurate operation of agricultural machinery
CN107607113B (en) Method for measuring inclination angles of two-axis attitude
CN110954102B (en) Magnetometer-assisted inertial navigation system and method for robot positioning
KR101922700B1 (en) Method and Apparatus for calculation of angular velocity using acceleration sensor and geomagnetic sensor
WO2011091083A1 (en) Apparatus and methodology for calibration of a gyroscope and a compass included in a handheld device
CN110779553A (en) Calibration method for magnetometer data
EP2721368A1 (en) Motion determination
JP2012173190A (en) Positioning system and positioning method
CN106813679B (en) Method and device for estimating attitude of moving object
CN116817896B (en) Gesture resolving method based on extended Kalman filtering
CN106403952A (en) Method for measuring combined attitudes of Satcom on the move with low cost
CN108731676A (en) A kind of posture fusion enhancing measurement method and system based on inertial navigation technology
CN107228674A (en) A kind of improved method for star sensor and gyro Federated filter
JP2019120587A (en) Positioning system and positioning method
CN111307114B (en) Measurement method of horizontal attitude of surface ship based on motion reference unit
JP6167747B2 (en) Bias calculation method and bias calculation apparatus
CN110375773B (en) Attitude initialization method for MEMS inertial navigation system
KR102598377B1 (en) Navigation instrument with tilt compensation and associated method
CN114812546A (en) Method and device for correcting position and attitude of individual soldier in occluded space
CN119469209B (en) Bionic polarized light compass and micro-inertial assisted three-axis magnetometer calibration method and device
CN116972833B (en) Horizontal attitude measurement system and method based on self-adaptive incremental Kalman filtering
CN114018279B (en) Multi-sampling-rate data fusion posture correction method for array sensor

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
WD01 Invention patent application deemed withdrawn after publication

Application publication date: 20200211

WD01 Invention patent application deemed withdrawn after publication