WO2022160391A1 - Magnetometer information assisted mems gyroscope calibration method and calibration system - Google Patents

Magnetometer information assisted mems gyroscope calibration method and calibration system Download PDF

Info

Publication number
WO2022160391A1
WO2022160391A1 PCT/CN2021/077089 CN2021077089W WO2022160391A1 WO 2022160391 A1 WO2022160391 A1 WO 2022160391A1 CN 2021077089 W CN2021077089 W CN 2021077089W WO 2022160391 A1 WO2022160391 A1 WO 2022160391A1
Authority
WO
WIPO (PCT)
Prior art keywords
magnetometer
mems
data
gyroscope
error
Prior art date
Application number
PCT/CN2021/077089
Other languages
French (fr)
Chinese (zh)
Inventor
徐祥
孙逸帆
李凤
陈洋豪
Original Assignee
苏州大学
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 苏州大学 filed Critical 苏州大学
Publication of WO2022160391A1 publication Critical patent/WO2022160391A1/en

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

Definitions

  • the invention relates to the technical field of inertial navigation systems, in particular to a MEMS gyroscope calibration method and a calibration system aided by magnetometer information.
  • the MEMS inertial device In order to ensure the accuracy of the inertial navigation system, the MEMS inertial device must be calibrated when it is used.
  • the traditional method usually uses a high-precision turntable to calibrate the gyroscope.
  • the use of a high-precision turntable to calibrate the MEMS gyroscope has problems such as high cost.
  • the output data of the MEMS accelerometer includes part of the external acceleration, and the external acceleration interferes with the calibration, which reduces the MEMS gyroscope. Calibration accuracy.
  • the technical problem to be solved by the present invention is to provide a magnetometer information-assisted MEMS gyroscope calibration method and calibration system, which utilizes the calibrated MEMS magnetometer data to calibrate the MEMS gyroscope and realizes low-cost MEMS gyroscope calibration.
  • the present invention provides a magnetometer information-assisted MEMS gyroscope calibration method, comprising the following steps:
  • the sensor real-time data includes real-time gyroscope data and magnetometer data
  • the real-time gyroscope data includes motion gyroscope data
  • the real-time gyroscope data also includes static gyroscope data; the interval between S1 and S3 also includes:
  • the noise covariance matrix in the initial conditions of the recursive least squares method is set according to the variance of the random noise of the MEMS gyroscope.
  • the interval between S1 and S3 also includes:
  • the ideal value of the MEMS gyroscope error parameter is set according to the MEMS gyroscope error parameter characteristic.
  • the interval between S1 and S3 also includes:
  • the recursive least squares method is used to iteratively estimate the parameters until the data is terminated, and the estimated value of the error parameter of the MEMS gyroscope is obtained, which specifically includes:
  • the ideal values of noise covariance matrix, error covariance matrix and MEMS gyroscope error estimation parameters are used as the iterative initial values of the recursive least squares equation, and the recursive least squares method is used to estimate the error parameters of the MEMS gyroscope until the data is terminated , to obtain an estimate of the error parameter of the MEMS gyroscope.
  • the S2 specifically includes;
  • b represents the carrier coordinate system, which represents the three-axis orthogonal coordinate system of the strapdown inertial navigation system, and its x-axis, y-axis and z-axis point to the right-front-upper of the carrier respectively;
  • s represents a non-orthogonal coordinate system, which represents a three-axis non-orthogonal coordinate system of the MEMS magnetometer, indicating that its x-axis, y-axis and z-axis have a certain angular deviation from the x-axis, y-axis and z-axis under the carrier coordinate system ;
  • n represents the navigation coordinate system, which represents the geographic coordinate system of the location of the carrier, and its three axes point to the local east, north and sky directions respectively;
  • the output model of the MEMS magnetometer in the non-orthogonal coordinate system is obtained:
  • m s represents the vector output of the MEMS magnetometer in a non-orthogonal system
  • S represents the scale factor error matrix of the MEMS magnetometer
  • C no represents the non-orthogonal error matrix of the MEMS magnetometer
  • m n represents the projection of the geomagnetic vector in the navigation coordinate system;
  • b represents the zero bias error vector of the MEMS magnetometer;
  • ⁇ represents the random noise vector of the MEMS magnetometer;
  • the maximum likelihood function F of the MEMS magnetometer is constructed under the optimal estimation model:
  • k represents the label of the measurement data, representing the kth measurement;
  • Re represents the kth vector output of the MEMS magnetometer in the non-orthogonal coordinate system;
  • S represents the scale factor error matrix of the MEMS magnetometer;
  • C no represents the non-orthogonal error matrix of the MEMS magnetometer;
  • b represents the zero bias error vector of the MEMS magnetometer;
  • ⁇ k represents the Lagrangian number;
  • the maximum likelihood function F of the MEMS magnetometer is estimated by Newton's method:
  • i represents the number of iterations of Newton's method, representing the ith iteration;
  • X (i+1) represents the estimated value of the MEMS magnetometer error parameter in the ith+1 iteration;
  • X (i) represents the ith iteration in Estimated values of MEMS magnetometer error parameters;
  • the error parameter value of the MEMS magnetometer is calculated by the Newton method, and the calibration model of the MEMS magnetometer is used for calibration:
  • m b represents the projection of the output of the MEMS magnetometer in the carrier coordinate system
  • R m represents the error compensation of the coupling term of the scale factor and the non-orthogonal error estimated by the maximum likelihood estimation method of the MEMS magnetometer matrix
  • b represents the compensation vector of the zero bias error estimated by the maximum likelihood estimation method.
  • the S3 specifically includes:
  • m b represents the projection of the geomagnetic vector in the carrier coordinate system;
  • ⁇ represents the vector product operation between vectors;
  • a column vector representing the component of the angular velocity vector of the carrier coordinate system relative to the local navigation coordinate system in the axial direction of the carrier system;
  • the recursive least squares method estimates MEMS gyroscope data errors, including:
  • K k P k
  • the S4 specifically includes:
  • R w represents the compensation matrix of the MEMS gyroscope scale factor and the non-orthogonal error coupling error;
  • d w represents the product of the MEMS gyroscope scale factor and the non-orthogonal error coupling error compensation matrix and the zero bias error vector, and ws represents the output of the MEMS gyroscope.
  • the invention discloses a MEMS gyroscope calibration system assisted by magnetometer information, comprising:
  • the data acquisition module acquires real-time sensor data
  • the sensor real-time data includes real-time gyroscope data and magnetometer data
  • the real-time gyroscope data includes motion gyroscope data
  • a magnetometer calibration module which performs calibration processing on the magnetometer data to obtain calibrated magnetometer data
  • the error parameter estimation module constructs an equation from the calibrated magnetometer data and the moving gyroscope data, uses the recursive least squares method to iteratively estimate the parameters until the data is terminated, and obtains the error parameters of the MEMS gyroscope estimated value;
  • a gyroscope calibration module performs calibration processing on the MEMS gyroscope according to the estimated value of the error parameter of the MEMS gyroscope.
  • the invention discloses a computer device, comprising a memory, a processor and a computer program stored in the memory and running on the processor, characterized in that the processor implements the steps of the above method when executing the program.
  • the present invention proposes a method for calibrating a MEMS gyroscope using a MEMS magnetometer as auxiliary information without the assistance of high-precision external equipment.
  • the present invention calibrates the MEMS magnetometer data and uses the calibrated MEMS
  • the magnetometer data is used to calibrate the MEMS gyroscope, which realizes the calibration of the low-cost MEMS gyroscope.
  • the present invention uses the full error estimation model based on the maximum likelihood method to calibrate the MEMS magnetometer, which improves the calibration accuracy of the MEMS magnetometer and further improves the calibration accuracy of the MEMS gyroscope;
  • the present invention uses the recursive least squares method to estimate the MEMS gyroscope, with high calculation accuracy and high calculation efficiency;
  • the present invention uses no high-precision external auxiliary equipment to calibrate the MEMS gyroscope, which saves the calibration cost and improves the practicability of the MEMS gyroscope.
  • Fig. 1 is the flow chart of the MEMS gyroscope calibration method aided by magnetometer information
  • Fig. 2 is the calibration bias error diagram of MEMS gyroscope
  • Fig. 3 is the first row vector error diagram of MEMS gyroscope calibration scale factor and non-orthogonal error coupling matrix
  • Fig. 4 is the error diagram of the second row vector of the MEMS gyroscope calibration scale factor and the non-orthogonal error coupling matrix
  • Fig. 5 is the third row vector error diagram of the MEMS gyroscope calibration scale factor and the non-orthogonal error coupling matrix.
  • the present invention discloses a magnetometer information-assisted MEMS gyroscope calibration method, including the following steps:
  • Step 1 Acquiring real-time sensor data, the sensor real-time data includes real-time gyroscope data and magnetometer data, and the real-time gyroscope data includes moving gyroscope data and stationary gyroscope data.
  • Step 2 Perform calibration processing on the magnetometer data to obtain the calibrated magnetometer data, which specifically includes:
  • b represents the carrier coordinate system, which represents the three-axis orthogonal coordinate system of the strapdown inertial navigation system, and its x-axis, y-axis and z-axis point to the right-front-upper of the carrier respectively;
  • s represents a non-orthogonal coordinate system, which represents a three-axis non-orthogonal coordinate system of the MEMS magnetometer, indicating that its x-axis, y-axis and z-axis have a certain angular deviation from the x-axis, y-axis and z-axis under the carrier coordinate system ;
  • n represents the navigation coordinate system, which represents the geographic coordinate system of the location of the carrier, and its three axes point to the local east, north and sky directions respectively;
  • the output model of the MEMS magnetometer in the non-orthogonal coordinate system is obtained:
  • m s represents the vector output of the MEMS magnetometer in a non-orthogonal system
  • S represents the scale factor error matrix of the MEMS magnetometer
  • C no represents the non-orthogonal error matrix of the MEMS magnetometer
  • m n represents the projection of the geomagnetic vector in the navigation coordinate system;
  • b represents the zero bias error vector of the MEMS magnetometer;
  • ⁇ represents the random noise vector of the MEMS magnetometer;
  • the maximum likelihood function F of the MEMS magnetometer is constructed under the optimal estimation model:
  • k represents the label of the measurement data, representing the kth measurement;
  • Re represents the kth vector output of the MEMS magnetometer in the non-orthogonal coordinate system;
  • S represents the scale factor error matrix of the MEMS magnetometer;
  • C no represents the non-orthogonal error matrix of the MEMS magnetometer;
  • b represents the zero bias error vector of the MEMS magnetometer;
  • ⁇ k represents the Lagrangian number;
  • the maximum likelihood function F of the MEMS magnetometer is estimated by Newton's method:
  • i represents the number of iterations of Newton's method, representing the ith iteration;
  • X (i+1) represents the estimated value of the MEMS magnetometer error parameter in the ith+1 iteration;
  • X (i) represents the ith iteration in Estimated values of MEMS magnetometer error parameters;
  • the error parameter value of the MEMS magnetometer is calculated by the Newton method, and the calibration model of the MEMS magnetometer is used for calibration:
  • m b represents the projection of the output of the MEMS magnetometer in the carrier coordinate system
  • R m represents the error compensation of the coupling term of the scale factor and the non-orthogonal error estimated by the maximum likelihood estimation method of the MEMS magnetometer matrix
  • b represents the compensation vector of the zero bias error estimated by the maximum likelihood estimation method.
  • the variance of the random noise of the MEMS gyroscope is obtained, and the noise covariance matrix in the initial condition of the recursive least squares method is set according to the variance of the random noise of the MEMS gyroscope; according to the MEMS gyroscope error parameter
  • the characteristic sets the ideal value of the MEMS gyroscope error parameter; the error covariance matrix is set according to the empirical value.
  • Step 3 the magnetometer data after calibration and the gyroscope data of motion construct equation, utilize the recursive least squares method to iteratively estimate the parameters until the data terminates, obtain the estimated value of the error parameter of the MEMS gyroscope, specifically include:
  • m b represents the projection of the geomagnetic vector in the carrier coordinate system;
  • ⁇ represents the vector product operation between vectors;
  • a column vector representing the component of the angular velocity vector of the carrier coordinate system relative to the local navigation coordinate system in the axial direction of the carrier system;
  • the recursive least squares method estimates MEMS gyroscope data errors, including:
  • K k P k
  • Step 4 Perform calibration processing on the MEMS gyroscope according to the estimated value of the error parameter of the MEMS gyroscope.
  • the recursive least squares method is used to iteratively estimate the parameters until the data is terminated, and the estimated value of the error parameter of the MEMS gyroscope is obtained, which specifically includes:
  • the ideal values of noise covariance matrix, error covariance matrix and MEMS gyroscope error estimation parameters are used as the iterative initial values of the recursive least squares equation, and the recursive least squares method is used to estimate the error parameters of the MEMS gyroscope until the data is terminated , to obtain the estimated value of the error parameter of the MEMS gyroscope, specifically including: MEMS gyroscope error calibration expression:
  • R w represents the compensation matrix of the MEMS gyroscope scale factor and the non-orthogonal error coupling error;
  • d w represents the product of the MEMS gyroscope scale factor and the non-orthogonal error coupling error compensation matrix and the zero-bias error vector;
  • ws represents the output of the MEMS gyroscope.
  • the invention discloses a MEMS gyroscope calibration system assisted by magnetometer information, which comprises a data acquisition module, a magnetometer calibration module, an error parameter estimation module and a gyroscope calibration module.
  • the data acquisition module acquires real-time sensor data, the sensor real-time data includes real-time gyroscope data and magnetometer data, and the real-time gyroscope data includes motion gyroscope data.
  • the magnetometer calibration module performs calibration processing on the magnetometer data to obtain calibrated magnetometer data.
  • the error parameter estimation module constructs an equation from the calibrated magnetometer data and the moving gyroscope data, uses the recursive least squares method to iteratively estimate the parameters until the data is terminated, and obtains the estimated value of the error parameter of the MEMS gyroscope.
  • the gyroscope calibration module performs calibration processing on the MEMS gyroscope according to the estimated value of the error parameter of the MEMS gyroscope.
  • the estimated MEMS gyroscope error parameters include two items, one is the zero bias error, and the other is the coupling term between the scale factor error and the non-orthogonal error.
  • Figure 2 shows the change of the difference between the estimated value of the zero bias error and the real value (the ideal result difference is 0) with the data iteration in the error parameters of the MEMS gyroscope, which is a 3x1 column vector (respectively x-axis, y-axis axis, z-axis direction), in the figure (a), (b), (c) respectively represent the difference between the estimated value of the zero bias error and the true value. It iterates with the data on the three axes x-axis, y-axis, and z-axis The change.
  • Figure 3 is a vector error diagram of the first row of the MEMS gyroscope calibration scale factor and the non-orthogonal error coupling matrix, wherein Figure 3 (a) shows the first row of the coupling term matrix between the scale factor and the non-orthogonal error. , the difference between the elements in the first column and the real value (simulation setting) changes with the data iteration; Figure 3(b) shows the coupling term matrix representing the scale factor and the non-orthogonal error. The first row, the difference between the elements in the second column and the real value (simulation setting) changes with the data iteration; Figure 3(c) shows the coupling term matrix representing the scale factor and the non-orthogonal error. One row, the difference between the element in the third column and the true value (simulation setting) as a function of data iteration.
  • Fig. 4 is a vector error diagram of the second row of the MEMS gyroscope calibration scale factor and the non-orthogonal error coupling matrix, in which Fig. 4(a) shows the second row of the coupling term matrix between the scale factor and the non-orthogonal error. , the difference between the elements in the first column and the real value (simulation setting) changes with the data iteration; Figure 4(b) shows the coupling term matrix representing the scale factor and the non-orthogonal error. The second row, the difference between the element in the second column and the real value (simulation setting) changes with the data iteration; Figure 4(c) shows the coupling term matrix representing the scale factor and the non-orthogonal error. The difference between the elements in the second row and the third column and the true value (simulation setting) varies with data iterations.
  • Figure 5 is a vector error diagram of the third row of the MEMS gyroscope calibration scale factor and the non-orthogonal error coupling matrix, wherein, 5(a) represents the third row of the coupling term matrix between the scale factor and the non-orthogonal error, The difference between the elements in the first column and the real value (simulation setting) changes with the data iteration; Figure 5(b) shows the coupling term matrix representing the scale factor and the non-orthogonal error. The third row , the difference between the elements in the second column and the real value (simulation setting) changes with the data iteration; Figure 5(c) shows the coupling term matrix representing the scale factor and the non-orthogonal error. The third row, the difference between the element in the third column and the true value (simulation setting) as a function of data iteration.
  • the difference between the MEMS gyroscope scale factor and the non-orthogonal error coupling term matrix obtained by the recursive least squares method and the real value (simulation setting) is minus 3 of 10 To the negative 4th power of 10, it shows that the error parameter estimation value obtained by the recursive least square method has low error and high accuracy.

Landscapes

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

Abstract

A magnetometer information assisted MEMS gyroscope calibration method and a calibration system. The method comprises the following steps: obtaining sensor real-time data, the sensor real-time data comprising real-time gyroscope data and magnetometer data, and the real-time gyroscope data comprising motion gyroscope data; performing calibration processing on the magnetometer data to obtain calibrated magnetometer data; establishing an equation by using the calibrated magnetometer data and the motion gyroscope data, and performing iterative estimation on parameters by using a recursive least square method until the data is terminated, so as to obtain an estimated value of an error parameter of an MEMS gyroscope; and performing calibration processing on the MEMS gyroscope according to the estimated value of the error parameter of the MEMS gyroscope. The MEMS gyroscope is calibrated by using the calibrated MEMS magnetometer data, thereby realizing low-cost MEMS gyroscope calibration.

Description

磁力计信息辅助的MEMS陀螺仪标定方法及标定系统Magnetometer information-aided MEMS gyroscope calibration method and calibration system 技术领域technical field
本发明涉及惯性导航系统技术领域，具体涉及一种磁力计信息辅助的MEMS陀螺仪标定方法及标定系统。The invention relates to the technical field of inertial navigation systems, in particular to a MEMS gyroscope calibration method and a calibration system aided by magnetometer information.
背景技术Background technique
为保证惯性导航系统精度，MEMS惯性器件在使用时，必须经过标定处理。对于MEMS陀螺仪标定，传统方法通常利用高精度转台对陀螺仪进行标定。但利用高精度转台对MEMS陀螺仪进行标定，存在成本过高等问题。近期，有学者提出利用MEMS加速度计数据作为辅助信息对MEMS磁力计进行标定，但在实际试验中，MEMS加速度计输出数据中包括部分外部加速度，外部加速度对标定存在干扰，降低了MEMS陀螺仪的标定精度。In order to ensure the accuracy of the inertial navigation system, the MEMS inertial device must be calibrated when it is used. For MEMS gyroscope calibration, the traditional method usually uses a high-precision turntable to calibrate the gyroscope. However, the use of a high-precision turntable to calibrate the MEMS gyroscope has problems such as high cost. Recently, some scholars proposed to use the MEMS accelerometer data as auxiliary information to calibrate the MEMS magnetometer. However, in the actual test, the output data of the MEMS accelerometer includes part of the external acceleration, and the external acceleration interferes with the calibration, which reduces the MEMS gyroscope. Calibration accuracy.
发明内容SUMMARY OF THE INVENTION
本发明要解决的技术问题是提供一种磁力计信息辅助的MEMS陀螺仪标定方法及标定系统，其利用标定后的MEMS磁力计数据对MEMS陀螺仪标定，实现了低成本MEMS陀螺仪标定。The technical problem to be solved by the present invention is to provide a magnetometer information-assisted MEMS gyroscope calibration method and calibration system, which utilizes the calibrated MEMS magnetometer data to calibrate the MEMS gyroscope and realizes low-cost MEMS gyroscope calibration.
为了解决上述技术问题，本发明提供了一种磁力计信息辅助的MEMS陀螺仪标定方法，包括以下步骤：In order to solve the above-mentioned technical problems, the present invention provides a magnetometer information-assisted MEMS gyroscope calibration method, comprising the following steps:
S1、获取传感器实时数据，所述传感器实时数据包括实时陀螺仪数据和磁力计数据,所述实时陀螺仪数据包括运动的陀螺仪数据；S1, acquire sensor real-time data, the sensor real-time data includes real-time gyroscope data and magnetometer data, and the real-time gyroscope data includes motion gyroscope data;
S2、对所述磁力计数据进行标定处理，获得标定后的磁力计数据；S2, performing calibration processing on the magnetometer data to obtain calibrated magnetometer data;
S3、标定后的磁力计数据与运动的陀螺仪数据构建方程，利用递推最小二乘法对参数迭代估计直至数据终止，获得MEMS陀螺仪的误差参数的估计值；S3. Construct an equation between the calibrated magnetometer data and the moving gyroscope data, use the recursive least squares method to iteratively estimate the parameters until the data is terminated, and obtain the estimated value of the error parameter of the MEMS gyroscope;
S4、根据MEMS陀螺仪的误差参数的估计值对MEMS陀螺仪进行标定处理。S4. Perform calibration processing on the MEMS gyroscope according to the estimated value of the error parameter of the MEMS gyroscope.
作为优选的，在S1中，所述实时陀螺仪数据还包括静止的陀螺仪数据；所述S1与S3之间还包括：Preferably, in S1, the real-time gyroscope data also includes static gyroscope data; the interval between S1 and S3 also includes:
根据静止的MEMS陀螺仪数据，获得MEMS陀螺仪随机噪声的方差大小；According to the static MEMS gyroscope data, the variance of the random noise of the MEMS gyroscope is obtained;
根据MEMS陀螺仪随机噪声的方差大小设定递推最小二乘法初始条件中噪声协方差矩阵。The noise covariance matrix in the initial conditions of the recursive least squares method is set according to the variance of the random noise of the MEMS gyroscope.
作为优选的，所述S1与S3之间还包括：Preferably, the interval between S1 and S3 also includes:
根据MEMS陀螺仪误差参数特性设定MEMS陀螺仪误差参数的理想值。The ideal value of the MEMS gyroscope error parameter is set according to the MEMS gyroscope error parameter characteristic.
作为优选的，所述S1与S3之间还包括：Preferably, the interval between S1 and S3 also includes:
根据经验值设定误差协方差矩阵。Set the error covariance matrix based on empirical values.
作为优选的，所述S3中利用递推最小二乘法对参数迭代估计直至数据终止，获得MEMS陀螺仪的误差参数的估计值，具体包括：Preferably, in the S3, the recursive least squares method is used to iteratively estimate the parameters until the data is terminated, and the estimated value of the error parameter of the MEMS gyroscope is obtained, which specifically includes:
将噪声协方差矩阵、误差协方差矩阵和MEMS陀螺仪误差估计参数的理想值作为递推最小二乘法方程的迭代初值，利用递推最小二乘法对MEMS陀螺仪的误差参数进行估计直至数据终止，获得MEMS陀螺仪的误差参数的估计值。The ideal values of noise covariance matrix, error covariance matrix and MEMS gyroscope error estimation parameters are used as the iterative initial values of the recursive least squares equation, and the recursive least squares method is used to estimate the error parameters of the MEMS gyroscope until the data is terminated , to obtain an estimate of the error parameter of the MEMS gyroscope.
作为优选的，所述S2具体包括；Preferably, the S2 specifically includes;
对获取的磁力计数据进行标定处理，定义标定磁力计所需参考坐标系：Perform calibration processing on the acquired magnetometer data, and define the reference coordinate system required for calibrating the magnetometer:
b代表载体坐标系，其表示捷联惯性导航系统三轴正交坐标系，其x轴、y轴和z轴分别指向载体的右-前-上；b represents the carrier coordinate system, which represents the three-axis orthogonal coordinate system of the strapdown inertial navigation system, and its x-axis, y-axis and z-axis point to the right-front-upper of the carrier respectively;
s代表非正交坐标系，其表示MEMS磁力计三轴非正交坐标系，表示其x轴、y轴和z轴与载体坐标系下的x轴、y轴和z轴存在一定的角度偏差；s represents a non-orthogonal coordinate system, which represents a three-axis non-orthogonal coordinate system of the MEMS magnetometer, indicating that its x-axis, y-axis and z-axis have a certain angular deviation from the x-axis, y-axis and z-axis under the carrier coordinate system ;
n代表导航坐标系，其表示载体所在位置的地理坐标系，其三轴分别指向当地东向、北向和天向；n represents the navigation coordinate system, which represents the geographic coordinate system of the location of the carrier, and its three axes point to the local east, north and sky directions respectively;
根据MEMS磁力计结构，得到MEMS磁力计在非正交坐标系下的输出模型：According to the structure of the MEMS magnetometer, the output model of the MEMS magnetometer in the non-orthogonal coordinate system is obtained:
Figure PCTCN2021077089-appb-000001
Figure PCTCN2021077089-appb-000001
其中，m s表示MEMS磁力计在非正交系下的矢量输出；S表示MEMS磁力计的标度因子误差矩阵；C no表示MEMS磁力计的非正交误差矩阵；
Figure PCTCN2021077089-appb-000002
表示由导航坐标系转向载体坐标系的方向余弦矩阵。m n表示地磁矢量在导航坐标系下的投影；b表示MEMS磁力计的零偏误差矢量；ε表示MEMS磁力计的随机噪声矢量； Among them, m s represents the vector output of the MEMS magnetometer in a non-orthogonal system; S represents the scale factor error matrix of the MEMS magnetometer; C no represents the non-orthogonal error matrix of the MEMS magnetometer;
Figure PCTCN2021077089-appb-000002
Represents the direction cosine matrix from the navigation coordinate system to the carrier coordinate system. m n represents the projection of the geomagnetic vector in the navigation coordinate system; b represents the zero bias error vector of the MEMS magnetometer; ε represents the random noise vector of the MEMS magnetometer;
根据MEMS磁力计在非正交坐标系下的输出，在最优估计模型下构建出MEMS磁力计极大似然函数F：According to the output of the MEMS magnetometer in the non-orthogonal coordinate system, the maximum likelihood function F of the MEMS magnetometer is constructed under the optimal estimation model:
Figure PCTCN2021077089-appb-000003
Figure PCTCN2021077089-appb-000003
其中，k表示测量数据的标签，代表第k次的测量；
Figure PCTCN2021077089-appb-000004
表示MEMS磁力计在非正交坐标系下第k次的矢量输出；S表示MEMS磁力计的标度因子误差矩阵；C no表示MEMS磁力计的非正交误差矩阵；
Figure PCTCN2021077089-appb-000005
表示地磁矢量在载体坐标系下第k次投影；b表示MEMS磁力计的零偏误差矢量；λ k表示拉格朗日常数； Among them, k represents the label of the measurement data, representing the kth measurement;
Figure PCTCN2021077089-appb-000004
Represents the kth vector output of the MEMS magnetometer in the non-orthogonal coordinate system; S represents the scale factor error matrix of the MEMS magnetometer; C no represents the non-orthogonal error matrix of the MEMS magnetometer;
Figure PCTCN2021077089-appb-000005
represents the kth projection of the geomagnetic vector in the carrier coordinate system; b represents the zero bias error vector of the MEMS magnetometer; λ k represents the Lagrangian number;
通过牛顿法对MEMS磁力计极大似然函数F进行估计：The maximum likelihood function F of the MEMS magnetometer is estimated by Newton's method:
Figure PCTCN2021077089-appb-000006
Figure PCTCN2021077089-appb-000006
其中，i表示牛顿法迭代次数，代表第i次的迭代；X (i+1)代表第i+1次迭代中 的MEMS磁力计误差参数估计值；X (i)代表第i次迭代中的MEMS磁力计误差参数估计值；
Figure PCTCN2021077089-appb-000007
代表第i次迭代中极大似然函数F的黑塞矩阵；
Figure PCTCN2021077089-appb-000008
代表第i次迭代中极大似然函数F的雅克比向量； Among them, i represents the number of iterations of Newton's method, representing the ith iteration; X (i+1) represents the estimated value of the MEMS magnetometer error parameter in the ith+1 iteration; X (i) represents the ith iteration in Estimated values of MEMS magnetometer error parameters;
Figure PCTCN2021077089-appb-000007
represents the Hessian matrix of the maximum likelihood function F in the ith iteration;
Figure PCTCN2021077089-appb-000008
represents the Jacobian vector of the maximum likelihood function F in the ith iteration;
通过牛顿法计算得出MEMS磁力计的误差参数值，利用MEMS磁力计标定模型进行标定：The error parameter value of the MEMS magnetometer is calculated by the Newton method, and the calibration model of the MEMS magnetometer is used for calibration:
m b＝R m(m s-b)， m b =R m (m s -b),
其中，m b表示MEMS磁力计的输出在载体坐标系下的投影；R m代表通过极大似然估计法估计出MEMS磁力计的，关于标度因子与非正交误差的耦合项的误差补偿矩阵；b表示通过极大似然估计法估计出的零偏误差的补偿向量。 Among them, m b represents the projection of the output of the MEMS magnetometer in the carrier coordinate system; R m represents the error compensation of the coupling term of the scale factor and the non-orthogonal error estimated by the maximum likelihood estimation method of the MEMS magnetometer matrix; b represents the compensation vector of the zero bias error estimated by the maximum likelihood estimation method.
作为优选的，所述S3具体包括：Preferably, the S3 specifically includes:
由哥氏定理获得：
Figure PCTCN2021077089-appb-000009
Obtained from Coriolis theorem:
Figure PCTCN2021077089-appb-000009
其中，
Figure PCTCN2021077089-appb-000010
表示地磁矢量在导航坐标系下的投影对于时间的微分；
Figure PCTCN2021077089-appb-000011
表示地磁矢量在载体坐标系下的投影对于时间的微分；
Figure PCTCN2021077089-appb-000012
表示载体坐标系相对当地导航坐标系的角速度矢量在载体系轴向的分量构成的列向量；×表示矢量间的向量积运算；m b表示地磁矢量在载体坐标系下的投影； in,
Figure PCTCN2021077089-appb-000010
Represents the time differential of the projection of the geomagnetic vector in the navigation coordinate system;
Figure PCTCN2021077089-appb-000011
Represents the differential of the projection of the geomagnetic vector in the carrier coordinate system with respect to time;
Figure PCTCN2021077089-appb-000012
represents the column vector formed by the component of the angular velocity vector of the carrier coordinate system relative to the local navigation coordinate system in the axial direction of the carrier system; × represents the vector product operation between the vectors; m b represents the projection of the geomagnetic vector under the carrier coordinate system;
由于地磁矢量在导航坐标系下的投影为常值，所以：
Figure PCTCN2021077089-appb-000013
Since the projection of the geomagnetic vector in the navigation coordinate system is constant, so:
Figure PCTCN2021077089-appb-000013
其中，
Figure PCTCN2021077089-appb-000014
表示地磁矢量在导航坐标系下的投影对于时间的微分；0表示零向量。 in,
Figure PCTCN2021077089-appb-000014
Represents the differential of the projection of the geomagnetic vector in the navigation coordinate system with respect to time; 0 represents the zero vector.
构建MEMS磁力计数据与MEMS陀螺仪数据的关系式：Construct the relationship between the MEMS magnetometer data and the MEMS gyroscope data:
Figure PCTCN2021077089-appb-000015
Figure PCTCN2021077089-appb-000015
其中，
Figure PCTCN2021077089-appb-000016
表示地磁矢量在载体坐标系下的投影对于时间的微分；m b表示地磁矢量在载体坐标系下的投影；×表示矢量间的向量积运算；
Figure PCTCN2021077089-appb-000017
表示载体坐标系相对当地导航坐标系的角速度矢量在载体系轴向的分量构成的列向量； in,
Figure PCTCN2021077089-appb-000016
Represents the differential of the projection of the geomagnetic vector in the carrier coordinate system with respect to time; m b represents the projection of the geomagnetic vector in the carrier coordinate system; × represents the vector product operation between vectors;
Figure PCTCN2021077089-appb-000017
A column vector representing the component of the angular velocity vector of the carrier coordinate system relative to the local navigation coordinate system in the axial direction of the carrier system;
递推最小二乘法估计MEMS陀螺仪数据误差，包括：The recursive least squares method estimates MEMS gyroscope data errors, including:
Figure PCTCN2021077089-appb-000018
Figure PCTCN2021077089-appb-000018
Figure PCTCN2021077089-appb-000019
Figure PCTCN2021077089-appb-000019
K k＝P k|k-1M k T(M kP k|k-1M k T+R k) -1， K k =P k|k-1 M k T (M k P k|k-1 M k T +R k ) -1 ,
Figure PCTCN2021077089-appb-000020
Figure PCTCN2021077089-appb-000020
P k|k＝(I-K kM k)P k|k-1， P k|k =(IK k M k )P k|k-1 ,
其中，
Figure PCTCN2021077089-appb-000021
表示k时刻递推最小二乘法一步预测；
Figure PCTCN2021077089-appb-000022
表示k-1时刻递推最小二乘法最优状态估计；
Figure PCTCN2021077089-appb-000023
表示k时刻经过标定后的MEMS磁力计在载体坐标系下的矢量输出对于时间的微分，表示k时刻量测；M k表示k时刻递推最小二乘估计中观测矩阵；K k表示k时刻的增益矩阵；P k|k-1表示k时刻一步预测状态误差协方差矩阵；R k表示k时刻量测噪声协方差矩阵；
Figure PCTCN2021077089-appb-000024
表示k时刻递推最小二乘法最优估计状态；P k|k表示k时刻的误差协方差矩阵；I表示单位矩阵。 in,
Figure PCTCN2021077089-appb-000021
Represents the one-step prediction of the recursive least squares method at time k;
Figure PCTCN2021077089-appb-000022
Represents the optimal state estimation of the recursive least squares method at time k-1;
Figure PCTCN2021077089-appb-000023
Represents the differential of the vector output of the calibrated MEMS magnetometer in the carrier coordinate system at time k with respect to time, and represents the measurement at time k; M k represents the observation matrix in the recursive least squares estimation at time k; K k represents the Gain matrix; P k|k-1 represents the one-step prediction state error covariance matrix at time k; R k represents the measurement noise covariance matrix at time k;
Figure PCTCN2021077089-appb-000024
Represents the optimal estimation state of the recursive least squares method at time k; P k|k represents the error covariance matrix at time k; I represents the identity matrix.
作为优选的，所述S4具体包括：Preferably, the S4 specifically includes:
MEMS陀螺仪误差标定表达式：MEMS gyroscope error calibration expression:
Figure PCTCN2021077089-appb-000025
Figure PCTCN2021077089-appb-000025
其中，
Figure PCTCN2021077089-appb-000026
表示载体坐标系相对当地导航坐标系的MEMS陀螺仪的输出角速度矢量在载体系轴向的分量构成的列向量；R w表示MEMS陀螺仪标度因子与非正交误差耦合误差的补偿矩阵；d w表示MEMS陀螺仪标度因子与非正交误差耦合误差的补偿矩阵与零偏误差向量的积，w s表示MEMS陀螺仪的输出。 in,
Figure PCTCN2021077089-appb-000026
Represents the column vector formed by the component of the output angular velocity vector of the MEMS gyroscope in the carrier coordinate system relative to the local navigation coordinate system in the axial direction of the carrier system; R w represents the compensation matrix of the MEMS gyroscope scale factor and the non-orthogonal error coupling error; d w represents the product of the MEMS gyroscope scale factor and the non-orthogonal error coupling error compensation matrix and the zero bias error vector, and ws represents the output of the MEMS gyroscope.
本发明公开了一种磁力计信息辅助的MEMS陀螺仪标定系统，包括：The invention discloses a MEMS gyroscope calibration system assisted by magnetometer information, comprising:
数据采集模块，所述数据采集模块获取传感器实时数据，所述传感器实时数据包括实时陀螺仪数据和磁力计数据,所述实时陀螺仪数据包括运动的陀螺仪数据；a data acquisition module, the data acquisition module acquires real-time sensor data, the sensor real-time data includes real-time gyroscope data and magnetometer data, and the real-time gyroscope data includes motion gyroscope data;
磁力计标定模块，所述磁力计标定模块对所述磁力计数据进行标定处理，获得标定后的磁力计数据；a magnetometer calibration module, which performs calibration processing on the magnetometer data to obtain calibrated magnetometer data;
误差参数估值模块，所述误差参数估值模块将标定后的磁力计数据与运动的陀螺仪数据构建方程，利用递推最小二乘法对参数迭代估计直至数据终止，获得MEMS陀螺仪的误差参数的估计值；Error parameter estimation module, the error parameter estimation module constructs an equation from the calibrated magnetometer data and the moving gyroscope data, uses the recursive least squares method to iteratively estimate the parameters until the data is terminated, and obtains the error parameters of the MEMS gyroscope estimated value;
陀螺仪标定模块，所述陀螺仪标定模块根据MEMS陀螺仪的误差参数的估计值对MEMS陀螺仪进行标定处理。A gyroscope calibration module, the gyroscope calibration module performs calibration processing on the MEMS gyroscope according to the estimated value of the error parameter of the MEMS gyroscope.
本发明公开了一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述程序时实现上述方法的步骤。The invention discloses a computer device, comprising a memory, a processor and a computer program stored in the memory and running on the processor, characterized in that the processor implements the steps of the above method when executing the program.
本发明的有益效果：Beneficial effects of the present invention:
1、本发明在无高精度外部设备辅助的情况下，提出了一种利用MEMS磁力 计作为辅助信息的MEMS陀螺仪标定的方法，本发明通过对MEMS磁力计数据进行标定，利用标定后的MEMS磁力计数据对MEMS陀螺仪标定，实现了低成本MEMS陀螺仪标定。1. The present invention proposes a method for calibrating a MEMS gyroscope using a MEMS magnetometer as auxiliary information without the assistance of high-precision external equipment. The present invention calibrates the MEMS magnetometer data and uses the calibrated MEMS The magnetometer data is used to calibrate the MEMS gyroscope, which realizes the calibration of the low-cost MEMS gyroscope.
2、本发明采用基于极大似然法的全误差估计模型对MEMS磁力计进行标定，提高了MEMS磁力计标定精度，进而提高了MEMS陀螺仪的标定精度；2. The present invention uses the full error estimation model based on the maximum likelihood method to calibrate the MEMS magnetometer, which improves the calibration accuracy of the MEMS magnetometer and further improves the calibration accuracy of the MEMS gyroscope;
3、本发明采用递推最小二乘法对MEMS陀螺仪进行估计，计算精度高，计算效率高；3. The present invention uses the recursive least squares method to estimate the MEMS gyroscope, with high calculation accuracy and high calculation efficiency;
4、本发明采用无高精度外部辅助设备对MEMS陀螺仪进行标定，节省了标定成本，提高了MEMS陀螺仪的实用性。4. The present invention uses no high-precision external auxiliary equipment to calibrate the MEMS gyroscope, which saves the calibration cost and improves the practicability of the MEMS gyroscope.
附图说明Description of drawings
图1为磁力计信息辅助的MEMS陀螺仪标定方法的流程图；Fig. 1 is the flow chart of the MEMS gyroscope calibration method aided by magnetometer information;
图2为MEMS陀螺仪标定零偏误差图；Fig. 2 is the calibration bias error diagram of MEMS gyroscope;
图3为MEMS陀螺仪标定标度因子与非正交误差耦合矩阵第一行向量误差图；Fig. 3 is the first row vector error diagram of MEMS gyroscope calibration scale factor and non-orthogonal error coupling matrix;
图4为MEMS陀螺仪标定标度因子与非正交误差耦合矩阵第二行向量误差图；Fig. 4 is the error diagram of the second row vector of the MEMS gyroscope calibration scale factor and the non-orthogonal error coupling matrix;
图5为MEMS陀螺仪标定标度因子与非正交误差耦合矩阵第三行向量误差图。Fig. 5 is the third row vector error diagram of the MEMS gyroscope calibration scale factor and the non-orthogonal error coupling matrix.
具体实施方式Detailed ways
下面结合附图和具体实施例对本发明作进一步说明，以使本领域的技术人员可以更好地理解本发明并能予以实施，但所举实施例不作为对本发明的限定。The present invention will be further described below with reference to the accompanying drawings and specific embodiments, so that those skilled in the art can better understand the present invention and implement it, but the embodiments are not intended to limit the present invention.
参照图1-图5所示，本发明的公开了一种磁力计信息辅助的MEMS陀螺仪标定方法，包括以下步骤：1-5, the present invention discloses a magnetometer information-assisted MEMS gyroscope calibration method, including the following steps:
步骤一、获取传感器实时数据，所述传感器实时数据包括实时陀螺仪数据和磁力计数据,所述实时陀螺仪数据包括运动的陀螺仪数据和静止的陀螺仪数据。Step 1: Acquiring real-time sensor data, the sensor real-time data includes real-time gyroscope data and magnetometer data, and the real-time gyroscope data includes moving gyroscope data and stationary gyroscope data.
步骤二、对所述磁力计数据进行标定处理，获得标定后的磁力计数据，具体包括：Step 2: Perform calibration processing on the magnetometer data to obtain the calibrated magnetometer data, which specifically includes:
对获取的磁力计数据进行标定处理，定义标定磁力计所需参考坐标系：Perform calibration processing on the acquired magnetometer data, and define the reference coordinate system required for calibrating the magnetometer:
b代表载体坐标系，其表示捷联惯性导航系统三轴正交坐标系，其x轴、y轴和z轴分别指向载体的右-前-上；b represents the carrier coordinate system, which represents the three-axis orthogonal coordinate system of the strapdown inertial navigation system, and its x-axis, y-axis and z-axis point to the right-front-upper of the carrier respectively;
s代表非正交坐标系，其表示MEMS磁力计三轴非正交坐标系，表示其x轴、y轴和z轴与载体坐标系下的x轴、y轴和z轴存在一定的角度偏差；s represents a non-orthogonal coordinate system, which represents a three-axis non-orthogonal coordinate system of the MEMS magnetometer, indicating that its x-axis, y-axis and z-axis have a certain angular deviation from the x-axis, y-axis and z-axis under the carrier coordinate system ;
n代表导航坐标系，其表示载体所在位置的地理坐标系，其三轴分别指向当地东向、北向和天向；n represents the navigation coordinate system, which represents the geographic coordinate system of the location of the carrier, and its three axes point to the local east, north and sky directions respectively;
根据MEMS磁力计结构，得到MEMS磁力计在非正交坐标系下的输出模型：According to the structure of the MEMS magnetometer, the output model of the MEMS magnetometer in the non-orthogonal coordinate system is obtained:
Figure PCTCN2021077089-appb-000027
Figure PCTCN2021077089-appb-000027
其中，m s表示MEMS磁力计在非正交系下的矢量输出；S表示MEMS磁力计的标度因子误差矩阵；C no表示MEMS磁力计的非正交误差矩阵；
Figure PCTCN2021077089-appb-000028
表示由导航坐标系转向载体坐标系的方向余弦矩阵。m n表示地磁矢量在导航坐标系下的投影；b表示MEMS磁力计的零偏误差矢量；ε表示MEMS磁力计的随机噪声矢量； Among them, m s represents the vector output of the MEMS magnetometer in a non-orthogonal system; S represents the scale factor error matrix of the MEMS magnetometer; C no represents the non-orthogonal error matrix of the MEMS magnetometer;
Figure PCTCN2021077089-appb-000028
Represents the direction cosine matrix from the navigation coordinate system to the carrier coordinate system. m n represents the projection of the geomagnetic vector in the navigation coordinate system; b represents the zero bias error vector of the MEMS magnetometer; ε represents the random noise vector of the MEMS magnetometer;
根据MEMS磁力计在非正交坐标系下的输出，在最优估计模型下构建出MEMS磁力计极大似然函数F：According to the output of the MEMS magnetometer in the non-orthogonal coordinate system, the maximum likelihood function F of the MEMS magnetometer is constructed under the optimal estimation model:
Figure PCTCN2021077089-appb-000029
Figure PCTCN2021077089-appb-000029
其中，k表示测量数据的标签，代表第k次的测量；
Figure PCTCN2021077089-appb-000030
表示MEMS磁力计在非正交坐标系下第k次的矢量输出；S表示MEMS磁力计的标度因子误差矩阵；C no表示MEMS磁力计的非正交误差矩阵；
Figure PCTCN2021077089-appb-000031
表示地磁矢量在载体坐标系下第k次投影；b表示MEMS磁力计的零偏误差矢量；λ k表示拉格朗日常数； Among them, k represents the label of the measurement data, representing the kth measurement;
Figure PCTCN2021077089-appb-000030
Represents the kth vector output of the MEMS magnetometer in the non-orthogonal coordinate system; S represents the scale factor error matrix of the MEMS magnetometer; C no represents the non-orthogonal error matrix of the MEMS magnetometer;
Figure PCTCN2021077089-appb-000031
represents the kth projection of the geomagnetic vector in the carrier coordinate system; b represents the zero bias error vector of the MEMS magnetometer; λ k represents the Lagrangian number;
通过牛顿法对MEMS磁力计极大似然函数F进行估计：The maximum likelihood function F of the MEMS magnetometer is estimated by Newton's method:
Figure PCTCN2021077089-appb-000032
Figure PCTCN2021077089-appb-000032
其中，i表示牛顿法迭代次数，代表第i次的迭代；X (i+1)代表第i+1次迭代中的MEMS磁力计误差参数估计值；X (i)代表第i次迭代中的MEMS磁力计误差参数估计值；
Figure PCTCN2021077089-appb-000033
代表第i次迭代中极大似然函数F的黑塞矩阵；
Figure PCTCN2021077089-appb-000034
代表第i次迭代中极大似然函数F的雅克比向量； Among them, i represents the number of iterations of Newton's method, representing the ith iteration; X (i+1) represents the estimated value of the MEMS magnetometer error parameter in the ith+1 iteration; X (i) represents the ith iteration in Estimated values of MEMS magnetometer error parameters;
Figure PCTCN2021077089-appb-000033
represents the Hessian matrix of the maximum likelihood function F in the ith iteration;
Figure PCTCN2021077089-appb-000034
represents the Jacobian vector of the maximum likelihood function F in the ith iteration;
通过牛顿法计算得出MEMS磁力计的误差参数值，利用MEMS磁力计标定模型进行标定：The error parameter value of the MEMS magnetometer is calculated by the Newton method, and the calibration model of the MEMS magnetometer is used for calibration:
m b＝R m(m s-b)， m b =R m (m s -b),
其中，m b表示MEMS磁力计的输出在载体坐标系下的投影；R m代表通过极大似然估计法估计出MEMS磁力计的，关于标度因子与非正交误差的耦合项的误差补偿矩阵；b表示通过极大似然估计法估计出的零偏误差的补偿向量。 Among them, m b represents the projection of the output of the MEMS magnetometer in the carrier coordinate system; R m represents the error compensation of the coupling term of the scale factor and the non-orthogonal error estimated by the maximum likelihood estimation method of the MEMS magnetometer matrix; b represents the compensation vector of the zero bias error estimated by the maximum likelihood estimation method.
之后，根据静止的MEMS陀螺仪数据，获得MEMS陀螺仪随机噪声的方差大小，根据MEMS陀螺仪随机噪声的方差大小设定递推最小二乘法初始条件中噪声协方差矩阵；根据MEMS陀螺仪误差参数特性设定MEMS陀螺仪误差参数的理想值；根据经验值设定误差协方差矩阵。After that, according to the static MEMS gyroscope data, the variance of the random noise of the MEMS gyroscope is obtained, and the noise covariance matrix in the initial condition of the recursive least squares method is set according to the variance of the random noise of the MEMS gyroscope; according to the MEMS gyroscope error parameter The characteristic sets the ideal value of the MEMS gyroscope error parameter; the error covariance matrix is set according to the empirical value.
步骤三、标定后的磁力计数据与运动的陀螺仪数据构建方程，利用递推最 小二乘法对参数迭代估计直至数据终止，获得MEMS陀螺仪的误差参数的估计值，具体包括：Step 3, the magnetometer data after calibration and the gyroscope data of motion construct equation, utilize the recursive least squares method to iteratively estimate the parameters until the data terminates, obtain the estimated value of the error parameter of the MEMS gyroscope, specifically include:
由哥氏定理获得：
Figure PCTCN2021077089-appb-000035
Obtained from Coriolis theorem:
Figure PCTCN2021077089-appb-000035
其中，
Figure PCTCN2021077089-appb-000036
表示地磁矢量在导航坐标系下的投影对于时间的微分；
Figure PCTCN2021077089-appb-000037
表示地磁矢量在载体坐标系下的投影对于时间的微分；
Figure PCTCN2021077089-appb-000038
表示载体坐标系相对当地导航坐标系的角速度矢量在载体系轴向的分量构成的列向量；×表示矢量间的向量积运算；m b表示地磁矢量在载体坐标系下的投影； in,
Figure PCTCN2021077089-appb-000036
Represents the time differential of the projection of the geomagnetic vector in the navigation coordinate system;
Figure PCTCN2021077089-appb-000037
Represents the differential of the projection of the geomagnetic vector in the carrier coordinate system with respect to time;
Figure PCTCN2021077089-appb-000038
represents the column vector formed by the component of the angular velocity vector of the carrier coordinate system relative to the local navigation coordinate system in the axial direction of the carrier system; × represents the vector product operation between the vectors; m b represents the projection of the geomagnetic vector under the carrier coordinate system;
由于地磁矢量在导航坐标系下的投影为常值，所以：
Figure PCTCN2021077089-appb-000039
Since the projection of the geomagnetic vector in the navigation coordinate system is constant, so:
Figure PCTCN2021077089-appb-000039
其中，
Figure PCTCN2021077089-appb-000040
表示地磁矢量在导航坐标系下的投影对于时间的微分；0表示零向量。 in,
Figure PCTCN2021077089-appb-000040
Represents the differential of the projection of the geomagnetic vector in the navigation coordinate system with respect to time; 0 represents the zero vector.
构建MEMS磁力计数据与MEMS陀螺仪数据的关系式：Construct the relationship between the MEMS magnetometer data and the MEMS gyroscope data:
Figure PCTCN2021077089-appb-000041
Figure PCTCN2021077089-appb-000041
其中，
Figure PCTCN2021077089-appb-000042
表示地磁矢量在载体坐标系下的投影对于时间的微分；m b表示地磁矢量在载体坐标系下的投影；×表示矢量间的向量积运算；
Figure PCTCN2021077089-appb-000043
表示载体坐标系相对当地导航坐标系的角速度矢量在载体系轴向的分量构成的列向量； in,
Figure PCTCN2021077089-appb-000042
Represents the differential of the projection of the geomagnetic vector in the carrier coordinate system with respect to time; m b represents the projection of the geomagnetic vector in the carrier coordinate system; × represents the vector product operation between vectors;
Figure PCTCN2021077089-appb-000043
A column vector representing the component of the angular velocity vector of the carrier coordinate system relative to the local navigation coordinate system in the axial direction of the carrier system;
递推最小二乘法估计MEMS陀螺仪数据误差，包括：The recursive least squares method estimates MEMS gyroscope data errors, including:
Figure PCTCN2021077089-appb-000044
Figure PCTCN2021077089-appb-000044
Figure PCTCN2021077089-appb-000045
Figure PCTCN2021077089-appb-000045
K k＝P k|k-1M k T(M kP k|k-1M k T+R k) -1， K k =P k|k-1 M k T (M k P k|k-1 M k T +R k ) -1 ,
Figure PCTCN2021077089-appb-000046
Figure PCTCN2021077089-appb-000046
P k|k＝(I-K kM k)P k|k-1， P k|k =(IK k M k )P k|k-1 ,
其中，
Figure PCTCN2021077089-appb-000047
表示k时刻递推最小二乘法一步预测；
Figure PCTCN2021077089-appb-000048
表示k-1时刻递推最小二乘法最优状态估计；
Figure PCTCN2021077089-appb-000049
表示k时刻经过标定后的MEMS磁力计在载体坐标系下的矢量输出对于时间的微分，表示k时刻量测；M k表示k时刻递推最小二乘估计中观测矩阵；K k表示k时刻的增益矩阵；P k|k-1表示k时刻一步预测状态误差协方差矩阵；R k表示k时刻量测噪声协方差矩阵；
Figure PCTCN2021077089-appb-000050
表示k时刻递推最小二乘法最优估计状态；P k|k表示k时刻的误差协方差矩阵；I表示单位矩阵。 in,
Figure PCTCN2021077089-appb-000047
Represents the one-step prediction of the recursive least squares method at time k;
Figure PCTCN2021077089-appb-000048
Represents the optimal state estimation of the recursive least squares method at time k-1;
Figure PCTCN2021077089-appb-000049
Represents the differential of the vector output of the calibrated MEMS magnetometer in the carrier coordinate system at time k with respect to time, and represents the measurement at time k; M k represents the observation matrix in the recursive least squares estimation at time k; K k represents the Gain matrix; P k|k-1 represents the one-step prediction state error covariance matrix at time k; R k represents the measurement noise covariance matrix at time k;
Figure PCTCN2021077089-appb-000050
Represents the optimal estimation state of the recursive least squares method at time k; P k|k represents the error covariance matrix at time k; I represents the identity matrix.
步骤四、根据MEMS陀螺仪的误差参数的估计值对MEMS陀螺仪进行标定处理。Step 4: Perform calibration processing on the MEMS gyroscope according to the estimated value of the error parameter of the MEMS gyroscope.
所述步骤三中利用递推最小二乘法对参数迭代估计直至数据终止，获得MEMS陀螺仪的误差参数的估计值，具体包括：In the step 3, the recursive least squares method is used to iteratively estimate the parameters until the data is terminated, and the estimated value of the error parameter of the MEMS gyroscope is obtained, which specifically includes:
将噪声协方差矩阵、误差协方差矩阵和MEMS陀螺仪误差估计参数的理想值作为递推最小二乘法方程的迭代初值，利用递推最小二乘法对MEMS陀螺仪的误差参数进行估计直至数据终止，获得MEMS陀螺仪的误差参数的估计值，具体包括：MEMS陀螺仪误差标定表达式：
Figure PCTCN2021077089-appb-000051
The ideal values of noise covariance matrix, error covariance matrix and MEMS gyroscope error estimation parameters are used as the iterative initial values of the recursive least squares equation, and the recursive least squares method is used to estimate the error parameters of the MEMS gyroscope until the data is terminated , to obtain the estimated value of the error parameter of the MEMS gyroscope, specifically including: MEMS gyroscope error calibration expression:
Figure PCTCN2021077089-appb-000051
其中，
Figure PCTCN2021077089-appb-000052
表示载体坐标系相对当地导航坐标系的MEMS陀螺仪的输出角速度矢量在载体系轴向的分量构成的列向量；R w表示MEMS陀螺仪标度因子与非正交误差耦合误差的补偿矩阵；d w表示MEMS陀螺仪标度因子与非正交误差耦合 误差的补偿矩阵与零偏误差向量的积；w s表示MEMS陀螺仪的输出。 in,
Figure PCTCN2021077089-appb-000052
Represents the column vector formed by the component of the output angular velocity vector of the MEMS gyroscope in the carrier coordinate system relative to the local navigation coordinate system in the axial direction of the carrier system; R w represents the compensation matrix of the MEMS gyroscope scale factor and the non-orthogonal error coupling error; d w represents the product of the MEMS gyroscope scale factor and the non-orthogonal error coupling error compensation matrix and the zero-bias error vector; ws represents the output of the MEMS gyroscope.
本发明公开了一种磁力计信息辅助的MEMS陀螺仪标定系统，包括数据采集模块、磁力计标定模块、误差参数估值模块和陀螺仪标定模块。The invention discloses a MEMS gyroscope calibration system assisted by magnetometer information, which comprises a data acquisition module, a magnetometer calibration module, an error parameter estimation module and a gyroscope calibration module.
所述数据采集模块获取传感器实时数据，所述传感器实时数据包括实时陀螺仪数据和磁力计数据,所述实时陀螺仪数据包括运动的陀螺仪数据。The data acquisition module acquires real-time sensor data, the sensor real-time data includes real-time gyroscope data and magnetometer data, and the real-time gyroscope data includes motion gyroscope data.
所述磁力计标定模块对所述磁力计数据进行标定处理，获得标定后的磁力计数据。The magnetometer calibration module performs calibration processing on the magnetometer data to obtain calibrated magnetometer data.
所述误差参数估值模块将标定后的磁力计数据与运动的陀螺仪数据构建方程，利用递推最小二乘法对参数迭代估计直至数据终止，获得MEMS陀螺仪的误差参数的估计值。The error parameter estimation module constructs an equation from the calibrated magnetometer data and the moving gyroscope data, uses the recursive least squares method to iteratively estimate the parameters until the data is terminated, and obtains the estimated value of the error parameter of the MEMS gyroscope.
所述陀螺仪标定模块根据MEMS陀螺仪的误差参数的估计值对MEMS陀螺仪进行标定处理。The gyroscope calibration module performs calibration processing on the MEMS gyroscope according to the estimated value of the error parameter of the MEMS gyroscope.
本发明中，估计的MEMS陀螺仪误差参数包括两项，一项为零偏误差，一项是标度因子误差与非正交误差的耦合项。In the present invention, the estimated MEMS gyroscope error parameters include two items, one is the zero bias error, and the other is the coupling term between the scale factor error and the non-orthogonal error.
图2表示的是MEMS陀螺仪误差参数中，零偏误差估计值与真实值间的差值(理想结果差值为0)随数据迭代的变化，为3x1的列向量(分别是x轴,y轴,z轴方向)，图中(a),(b),(c)分别表示零偏误差估计值与真实值间的差值在三个轴x轴,y轴,z轴上随数据迭代的变化。由图2可以看出，经过递推最小二乘法得到的MEMS陀螺仪零偏误差的估计值与真实值(仿真设定)的差值小于0.01deg/s，说明我们递推最小二乘法估计得到的MEMS陀螺仪零偏误差较为准确，效果较好。Figure 2 shows the change of the difference between the estimated value of the zero bias error and the real value (the ideal result difference is 0) with the data iteration in the error parameters of the MEMS gyroscope, which is a 3x1 column vector (respectively x-axis, y-axis axis, z-axis direction), in the figure (a), (b), (c) respectively represent the difference between the estimated value of the zero bias error and the true value. It iterates with the data on the three axes x-axis, y-axis, and z-axis The change. It can be seen from Figure 2 that the difference between the estimated value of the zero bias error of the MEMS gyroscope obtained by the recursive least squares method and the real value (simulation setting) is less than 0.01deg/s, indicating that we can obtain the estimated value by the recursive least squares method. The zero bias error of the MEMS gyroscope is more accurate and the effect is better.
图3为MEMS陀螺仪标定标度因子与非正交误差耦合矩阵第一行向量误差图，其中，图3(a)中表示的是标度因子与非正交误差的耦合项矩阵第一行，第 一列的元素与真实值(仿真设定)间的差值随数据迭代的变化；图3(b)中表示的是表示的是标度因子与非正交误差的耦合项矩阵第一行，第二列的元素与真实值(仿真设定)间的差值随数据迭代的变化；图3(c)中表示的是表示的是标度因子与非正交误差的耦合项矩阵第一行，第三列的元素与真实值(仿真设定)间的差值随数据迭代的变化。Figure 3 is a vector error diagram of the first row of the MEMS gyroscope calibration scale factor and the non-orthogonal error coupling matrix, wherein Figure 3 (a) shows the first row of the coupling term matrix between the scale factor and the non-orthogonal error. , the difference between the elements in the first column and the real value (simulation setting) changes with the data iteration; Figure 3(b) shows the coupling term matrix representing the scale factor and the non-orthogonal error. The first row, the difference between the elements in the second column and the real value (simulation setting) changes with the data iteration; Figure 3(c) shows the coupling term matrix representing the scale factor and the non-orthogonal error. One row, the difference between the element in the third column and the true value (simulation setting) as a function of data iteration.
图4为MEMS陀螺仪标定标度因子与非正交误差耦合矩阵第二行向量误差图，其中，图4(a)中表示的是标度因子与非正交误差的耦合项矩阵第二行，第一列的元素与真实值(仿真设定)间的差值随数据迭代的变化；图4(b)中表示的是表示的是标度因子与非正交误差的耦合项矩阵第二行，第二列的元素与真实值(仿真设定)间的差值随数据迭代的变化；图4(c)中表示的是表示的是标度因子与非正交误差的耦合项矩阵第二行，第三列的元素与真实值(仿真设定)间的差值随数据迭代的变化。Fig. 4 is a vector error diagram of the second row of the MEMS gyroscope calibration scale factor and the non-orthogonal error coupling matrix, in which Fig. 4(a) shows the second row of the coupling term matrix between the scale factor and the non-orthogonal error. , the difference between the elements in the first column and the real value (simulation setting) changes with the data iteration; Figure 4(b) shows the coupling term matrix representing the scale factor and the non-orthogonal error. The second row, the difference between the element in the second column and the real value (simulation setting) changes with the data iteration; Figure 4(c) shows the coupling term matrix representing the scale factor and the non-orthogonal error. The difference between the elements in the second row and the third column and the true value (simulation setting) varies with data iterations.
图5为MEMS陀螺仪标定标度因子与非正交误差耦合矩阵第三行向量误差图，其中，5(a)中表示的是标度因子与非正交误差的耦合项矩阵第三行，第一列的元素与真实值(仿真设定)间的差值随数据迭代的变化；图5(b)中表示的是表示的是标度因子与非正交误差的耦合项矩阵第三行，第二列的元素与真实值(仿真设定)间的差值随数据迭代的变化；图5(c)中表示的是表示的是标度因子与非正交误差的耦合项矩阵第三行，第三列的元素与真实值(仿真设定)间的差值随数据迭代的变化。Figure 5 is a vector error diagram of the third row of the MEMS gyroscope calibration scale factor and the non-orthogonal error coupling matrix, wherein, 5(a) represents the third row of the coupling term matrix between the scale factor and the non-orthogonal error, The difference between the elements in the first column and the real value (simulation setting) changes with the data iteration; Figure 5(b) shows the coupling term matrix representing the scale factor and the non-orthogonal error. The third row , the difference between the elements in the second column and the real value (simulation setting) changes with the data iteration; Figure 5(c) shows the coupling term matrix representing the scale factor and the non-orthogonal error. The third row, the difference between the element in the third column and the true value (simulation setting) as a function of data iteration.
从图3-图5可看出，经过递推最小二乘法得到的MEMS陀螺仪标度因子和非正交误差耦合项矩阵各项与真实值(仿真设定)的差值在10的负3至10的负4次方量级，说明经过递推最小二乘法得到的误差参数估计值误差低，准确度高。As can be seen from Figure 3-5, the difference between the MEMS gyroscope scale factor and the non-orthogonal error coupling term matrix obtained by the recursive least squares method and the real value (simulation setting) is minus 3 of 10 To the negative 4th power of 10, it shows that the error parameter estimation value obtained by the recursive least square method has low error and high accuracy.
以上所述实施例仅是为充分说明本发明而所举的较佳的实施例，本发明的保护范围不限于此。本技术领域的技术人员在本发明基础上所作的等同替代或 变换，均在本发明的保护范围之内。本发明的保护范围以权利要求书为准。The above-mentioned embodiments are only preferred embodiments for fully illustrating the present invention, and the protection scope of the present invention is not limited thereto. Equivalent substitutions or transformations made by those skilled in the art on the basis of the present invention are all within the protection scope of the present invention. The protection scope of the present invention is subject to the claims.

Claims (10)

  1. 一种磁力计信息辅助的MEMS陀螺仪标定方法，其特征在于，包括以下步骤：A method for calibrating a MEMS gyroscope assisted by magnetometer information, comprising the following steps:
    S1、获取传感器实时数据，所述传感器实时数据包括实时陀螺仪数据和磁力计数据,所述实时陀螺仪数据包括运动的陀螺仪数据；S1, acquire sensor real-time data, the sensor real-time data includes real-time gyroscope data and magnetometer data, and the real-time gyroscope data includes motion gyroscope data;
    S2、对所述磁力计数据进行标定处理，获得标定后的磁力计数据；S2, performing calibration processing on the magnetometer data to obtain calibrated magnetometer data;
    S3、标定后的磁力计数据与运动的陀螺仪数据构建方程，利用递推最小二乘法对参数迭代估计直至数据终止，获得MEMS陀螺仪的误差参数的估计值；S3. Construct an equation between the calibrated magnetometer data and the moving gyroscope data, use the recursive least squares method to iteratively estimate the parameters until the data is terminated, and obtain the estimated value of the error parameter of the MEMS gyroscope;
    S4、根据MEMS陀螺仪的误差参数的估计值对MEMS陀螺仪进行标定处理。S4. Perform calibration processing on the MEMS gyroscope according to the estimated value of the error parameter of the MEMS gyroscope.
  2. 如权利要求1所述的磁力计信息辅助的MEMS陀螺仪标定方法，其特征在于，在S1中，所述实时陀螺仪数据还包括静止的陀螺仪数据；所述S1与S3之间还包括：The method for calibrating a MEMS gyroscope assisted by magnetometer information according to claim 1, wherein in S1, the real-time gyroscope data further includes static gyroscope data; and between the S1 and S3 further includes:
    根据静止的MEMS陀螺仪数据，获得MEMS陀螺仪随机噪声的方差大小；According to the static MEMS gyroscope data, the variance of the random noise of the MEMS gyroscope is obtained;
    根据MEMS陀螺仪随机噪声的方差大小设定递推最小二乘法初始条件中噪声协方差矩阵。The noise covariance matrix in the initial conditions of the recursive least squares method is set according to the variance of the random noise of the MEMS gyroscope.
  3. 如权利要求2所述的磁力计信息辅助的MEMS陀螺仪标定方法，其特征在于，所述S1与S3之间还包括：The method for calibrating a MEMS gyroscope assisted by magnetometer information according to claim 2, wherein the interval between S1 and S3 further comprises:
    根据MEMS陀螺仪误差参数特性设定MEMS陀螺仪误差参数的理想值。The ideal value of the MEMS gyroscope error parameter is set according to the MEMS gyroscope error parameter characteristic.
  4. 如权利要求3所述的磁力计信息辅助的MEMS陀螺仪标定方法，其特征在于，所述S1与S3之间还包括：The magnetometer information-assisted MEMS gyroscope calibration method according to claim 3, wherein the distance between S1 and S3 further comprises:
    根据经验值设定误差协方差矩阵。Set the error covariance matrix based on empirical values.
  5. 如权利要求4所述的磁力计信息辅助的MEMS陀螺仪标定方法，其特征在于，所述S3中利用递推最小二乘法对参数迭代估计直至数据终止，获得MEMS陀螺仪的误差参数的估计值，具体包括：The method for calibrating a MEMS gyroscope assisted by magnetometer information according to claim 4, wherein in said S3, the recursive least squares method is used to iteratively estimate the parameters until the data is terminated, and the estimated value of the error parameter of the MEMS gyroscope is obtained. , including:
    将噪声协方差矩阵、误差协方差矩阵和MEMS陀螺仪误差估计参数的理想值作为递推最小二乘法方程的迭代初值，利用递推最小二乘法对MEMS陀螺仪的误差参数进行估计直至数据终止，获得MEMS陀螺仪的误差参数的估计值。The ideal values of noise covariance matrix, error covariance matrix and MEMS gyroscope error estimation parameters are used as the iterative initial values of the recursive least squares equation, and the recursive least squares method is used to estimate the error parameters of the MEMS gyroscope until the data is terminated , to obtain an estimate of the error parameter of the MEMS gyroscope.
  6. 如权利要求1所述的磁力计信息辅助的MEMS陀螺仪标定方法，其特征在于，所述S2具体包括；The magnetometer information-assisted MEMS gyroscope calibration method according to claim 1, wherein the S2 specifically includes;
    对获取的磁力计数据进行标定处理，定义标定磁力计所需参考坐标系：Perform calibration processing on the acquired magnetometer data, and define the reference coordinate system required for calibrating the magnetometer:
    b代表载体坐标系，其表示捷联惯性导航系统三轴正交坐标系，其x轴、y轴和z轴分别指向载体的右-前-上；b represents the carrier coordinate system, which represents the three-axis orthogonal coordinate system of the strapdown inertial navigation system, and its x-axis, y-axis and z-axis point to the right-front-upper of the carrier respectively;
    s代表非正交坐标系，其表示MEMS磁力计三轴非正交坐标系，表示其x轴、y轴和z轴与载体坐标系下的x轴、y轴和z轴存在一定的角度偏差；s represents a non-orthogonal coordinate system, which represents a three-axis non-orthogonal coordinate system of the MEMS magnetometer, indicating that its x-axis, y-axis and z-axis have a certain angular deviation from the x-axis, y-axis and z-axis under the carrier coordinate system ;
    n代表导航坐标系，其表示载体所在位置的地理坐标系，其三轴分别指向当地东向、北向和天向；n represents the navigation coordinate system, which represents the geographic coordinate system of the location of the carrier, and its three axes point to the local east, north and sky directions respectively;
    根据MEMS磁力计结构，得到MEMS磁力计在非正交坐标系下的输出模型：According to the structure of the MEMS magnetometer, the output model of the MEMS magnetometer in the non-orthogonal coordinate system is obtained:
    Figure PCTCN2021077089-appb-100001
    Figure PCTCN2021077089-appb-100001
    其中，m s表示MEMS磁力计在非正交系下的矢量输出；S表示MEMS磁力计的标度因子误差矩阵；C no表示MEMS磁力计的非正交误差矩阵；
    Figure PCTCN2021077089-appb-100002
    表示由导航坐标系转向载体坐标系的方向余弦矩阵。m n表示地磁矢量在导航坐标系下的投影；b表示MEMS磁力计的零偏误差矢量；ε表示MEMS磁力计的随机噪声矢量；     Among them, m s represents the vector output of the MEMS magnetometer in a non-orthogonal system; S represents the scale factor error matrix of the MEMS magnetometer; C no represents the non-orthogonal error matrix of the MEMS magnetometer;
    Figure PCTCN2021077089-appb-100002
    Represents the direction cosine matrix from the navigation coordinate system to the carrier coordinate system. m n represents the projection of the geomagnetic vector in the navigation coordinate system; b represents the zero bias error vector of the MEMS magnetometer; ε represents the random noise vector of the MEMS magnetometer;
    根据MEMS磁力计在非正交坐标系下的输出，在最优估计模型下构建出MEMS 磁力计极大似然函数F：According to the output of the MEMS magnetometer in the non-orthogonal coordinate system, the maximum likelihood function F of the MEMS magnetometer is constructed under the optimal estimation model:
    Figure PCTCN2021077089-appb-100003
    Figure PCTCN2021077089-appb-100003
    其中，k表示测量数据的标签，代表第k次的测量；
    Figure PCTCN2021077089-appb-100004
    表示MEMS磁力计在非正交坐标系下第k次的矢量输出；S表示MEMS磁力计的标度因子误差矩阵；C no表示MEMS磁力计的非正交误差矩阵；
    Figure PCTCN2021077089-appb-100005
    表示地磁矢量在载体坐标系下第k次投影；b表示MEMS磁力计的零偏误差矢量；λ k表示拉格朗日常数；     Among them, k represents the label of the measurement data, representing the kth measurement;
    Figure PCTCN2021077089-appb-100004
    Represents the kth vector output of the MEMS magnetometer in the non-orthogonal coordinate system; S represents the scale factor error matrix of the MEMS magnetometer; C no represents the non-orthogonal error matrix of the MEMS magnetometer;
    Figure PCTCN2021077089-appb-100005
    represents the kth projection of the geomagnetic vector in the carrier coordinate system; b represents the zero bias error vector of the MEMS magnetometer; λ k represents the Lagrangian number;
    通过牛顿法对MEMS磁力计极大似然函数F进行估计：The maximum likelihood function F of the MEMS magnetometer is estimated by Newton's method:
    Figure PCTCN2021077089-appb-100006
    Figure PCTCN2021077089-appb-100006
    其中，i表示牛顿法迭代次数，代表第i次的迭代；X (i+1)代表第i+1次迭代中的MEMS磁力计误差参数估计值；X (i)代表第i次迭代中的MEMS磁力计误差参数估计值；
    Figure PCTCN2021077089-appb-100007
    代表第i次迭代中极大似然函数F的黑塞矩阵；
    Figure PCTCN2021077089-appb-100008
    代表第i次迭代中极大似然函数F的雅克比向量；     Among them, i represents the number of iterations of Newton's method, representing the ith iteration; X (i+1) represents the estimated value of the MEMS magnetometer error parameter in the ith+1 iteration; X (i) represents the ith iteration in Estimated values of MEMS magnetometer error parameters;
    Figure PCTCN2021077089-appb-100007
    represents the Hessian matrix of the maximum likelihood function F in the ith iteration;
    Figure PCTCN2021077089-appb-100008
    represents the Jacobian vector of the maximum likelihood function F in the ith iteration;
    通过牛顿法计算得出MEMS磁力计的误差参数值，利用MEMS磁力计标定模型进行标定：The error parameter value of the MEMS magnetometer is calculated by the Newton method, and the calibration model of the MEMS magnetometer is used for calibration:
    m b＝R m(m s-b)， m b =R m (m s -b),
    其中，m b表示MEMS磁力计的输出在载体坐标系下的投影；R m代表通过极大似然估计法估计出MEMS磁力计的，关于标度因子与非正交误差的耦合项的误差补偿矩阵；b表示通过极大似然估计法估计出的零偏误差的补偿向量。 Among them, m b represents the projection of the output of the MEMS magnetometer in the carrier coordinate system; R m represents the error compensation of the coupling term of the scale factor and the non-orthogonal error estimated by the maximum likelihood estimation method of the MEMS magnetometer matrix; b represents the compensation vector of the zero bias error estimated by the maximum likelihood estimation method.
  7. 如权利要求1所述的磁力计信息辅助的MEMS陀螺仪标定方法，其特征在于，所述S3具体包括：The magnetometer information-assisted MEMS gyroscope calibration method according to claim 1, wherein the S3 specifically comprises:
    由哥氏定理获得：
    Figure PCTCN2021077089-appb-100009
    Obtained from Coriolis theorem:
    Figure PCTCN2021077089-appb-100009
    其中，
    Figure PCTCN2021077089-appb-100010
    表示地磁矢量在导航坐标系下的投影对于时间的微分；
    Figure PCTCN2021077089-appb-100011
    表示地磁矢量在载体坐标系下的投影对于时间的微分；
    Figure PCTCN2021077089-appb-100012
    表示载体坐标系相对当地导航坐标系的角速度矢量在载体系轴向的分量构成的列向量；×表示矢量间的向量积运算；m b表示地磁矢量在载体坐标系下的投影；     in,
    Figure PCTCN2021077089-appb-100010
    Represents the time differential of the projection of the geomagnetic vector in the navigation coordinate system;
    Figure PCTCN2021077089-appb-100011
    Represents the differential of the projection of the geomagnetic vector in the carrier coordinate system with respect to time;
    Figure PCTCN2021077089-appb-100012
    represents the column vector formed by the component of the angular velocity vector of the carrier coordinate system relative to the local navigation coordinate system in the axial direction of the carrier system; × represents the vector product operation between the vectors; m b represents the projection of the geomagnetic vector under the carrier coordinate system;
    由于地磁矢量在导航坐标系下的投影为常值，所以：
    Figure PCTCN2021077089-appb-100013
    Since the projection of the geomagnetic vector in the navigation coordinate system is constant, so:
    Figure PCTCN2021077089-appb-100013
    其中，
    Figure PCTCN2021077089-appb-100014
    表示地磁矢量在导航坐标系下的投影对于时间的微分；0表示零向量。     in,
    Figure PCTCN2021077089-appb-100014
    Represents the differential of the projection of the geomagnetic vector in the navigation coordinate system with respect to time; 0 represents the zero vector.
    构建MEMS磁力计数据与MEMS陀螺仪数据的关系式：Construct the relationship between the MEMS magnetometer data and the MEMS gyroscope data:
    Figure PCTCN2021077089-appb-100015
    Figure PCTCN2021077089-appb-100015
    其中，
    Figure PCTCN2021077089-appb-100016
    表示地磁矢量在载体坐标系下的投影对于时间的微分；m b表示地磁矢量在载体坐标系下的投影；×表示矢量间的向量积运算；
    Figure PCTCN2021077089-appb-100017
    表示载体坐标系相对当地导航坐标系的角速度矢量在载体系轴向的分量构成的列向量；     in,
    Figure PCTCN2021077089-appb-100016
    Represents the differential of the projection of the geomagnetic vector in the carrier coordinate system with respect to time; m b represents the projection of the geomagnetic vector in the carrier coordinate system; × represents the vector product operation between vectors;
    Figure PCTCN2021077089-appb-100017
    A column vector representing the component of the angular velocity vector of the carrier coordinate system relative to the local navigation coordinate system in the axial direction of the carrier system;
    递推最小二乘法估计MEMS陀螺仪数据误差，包括：The recursive least squares method estimates MEMS gyroscope data errors, including:
    Figure PCTCN2021077089-appb-100018
    Figure PCTCN2021077089-appb-100018
    Figure PCTCN2021077089-appb-100019
    Figure PCTCN2021077089-appb-100019
    K k＝P k|k-1M k T(M kP k|k-1M k T+R k) -1， K k =P k|k-1 M k T (M k P k|k-1 M k T +R k ) -1 ,
    Figure PCTCN2021077089-appb-100020
    Figure PCTCN2021077089-appb-100020
    P k|k＝(I-K kM k)P k|k-1， P k|k =(IK k M k )P k|k-1 ,
    其中，
    Figure PCTCN2021077089-appb-100021
    表示k时刻递推最小二乘法一步预测；
    Figure PCTCN2021077089-appb-100022
    表示k-1时刻递推最小二乘法最优状态估计；
    Figure PCTCN2021077089-appb-100023
    表示k时刻经过标定后的MEMS磁力计在载体坐标系下的矢量输出对于时间的微分，表示k时刻量测；M k表示k时刻递推最小二乘估计中观测矩阵；K k表示k时刻的增益矩阵；P k|k-1表示k时刻一步预测状态误差协方差矩阵；R k表示k时刻量测噪声协方差矩阵；
    Figure PCTCN2021077089-appb-100024
    表示k时刻递推最小二乘法最优估计状态；P k|k表示k时刻的误差协方差矩阵；I表示单位矩阵。     in,
    Figure PCTCN2021077089-appb-100021
    Represents the one-step prediction of the recursive least squares method at time k;
    Figure PCTCN2021077089-appb-100022
    Represents the optimal state estimation of the recursive least squares method at time k-1;
    Figure PCTCN2021077089-appb-100023
    Represents the differential of the vector output of the calibrated MEMS magnetometer in the carrier coordinate system at time k with respect to time, and represents the measurement at time k; M k represents the observation matrix in the recursive least squares estimation at time k; K k represents the Gain matrix; P k|k-1 represents the one-step prediction state error covariance matrix at time k; R k represents the measurement noise covariance matrix at time k;
    Figure PCTCN2021077089-appb-100024
    Represents the optimal estimation state of the recursive least squares method at time k; P k|k represents the error covariance matrix at time k; I represents the identity matrix.
  8. 如权利要求1所述的磁力计信息辅助的MEMS陀螺仪标定方法，其特征在于，所述S4具体包括：The magnetometer information-assisted MEMS gyroscope calibration method according to claim 1, wherein the S4 specifically includes:
    MEMS陀螺仪误差标定表达式：MEMS gyroscope error calibration expression:
    Figure PCTCN2021077089-appb-100025
    Figure PCTCN2021077089-appb-100025
    其中，
    Figure PCTCN2021077089-appb-100026
    表示载体坐标系相对当地导航坐标系的MEMS陀螺仪的输出角速度矢量在载体系轴向的分量构成的列向量；R w表示MEMS陀螺仪标度因子与非正交误差耦合误差的补偿矩阵；d w表示MEMS陀螺仪标度因子与非正交误差耦合误差的补偿矩阵与零偏误差向量的积，w s表示MEMS陀螺仪的输出。     in,
    Figure PCTCN2021077089-appb-100026
    Represents the column vector formed by the component of the output angular velocity vector of the MEMS gyroscope in the carrier coordinate system relative to the local navigation coordinate system in the axial direction of the carrier system; R w represents the compensation matrix of the MEMS gyroscope scale factor and the non-orthogonal error coupling error; d w represents the product of the MEMS gyroscope scale factor and the non-orthogonal error coupling error compensation matrix and the zero bias error vector, and ws represents the output of the MEMS gyroscope.
  9. 一种磁力计信息辅助的MEMS陀螺仪标定系统，其特征在于,包括：A magnetometer information-assisted MEMS gyroscope calibration system, characterized in that, comprising:
    数据采集模块，所述数据采集模块获取传感器实时数据，所述传感器实时数据包括实时陀螺仪数据和磁力计数据,所述实时陀螺仪数据包括运动的陀螺仪数据；a data acquisition module, the data acquisition module acquires real-time sensor data, the sensor real-time data includes real-time gyroscope data and magnetometer data, and the real-time gyroscope data includes motion gyroscope data;
    磁力计标定模块，所述磁力计标定模块对所述磁力计数据进行标定处理，获得标定后的磁力计数据；a magnetometer calibration module, which performs calibration processing on the magnetometer data to obtain calibrated magnetometer data;
    误差参数估值模块，所述误差参数估值模块将标定后的磁力计数据与运动的陀螺仪数据构建方程，利用递推最小二乘法对参数迭代估计直至数据终止，获得MEMS陀螺仪的误差参数的估计值；Error parameter estimation module, the error parameter estimation module constructs an equation from the calibrated magnetometer data and the moving gyroscope data, uses the recursive least squares method to iteratively estimate the parameters until the data is terminated, and obtains the error parameters of the MEMS gyroscope estimated value;
    陀螺仪标定模块，所述陀螺仪标定模块根据MEMS陀螺仪的误差参数的估计值对MEMS陀螺仪进行标定处理。A gyroscope calibration module, the gyroscope calibration module performs calibration processing on the MEMS gyroscope according to the estimated value of the error parameter of the MEMS gyroscope.
  10. 一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述程序时实现权利要求1到8任一项所述方法的步骤。A computer device, comprising a memory, a processor and a computer program stored in the memory and running on the processor, characterized in that, when the processor executes the program, any one of claims 1 to 8 is implemented steps of the method.
PCT/CN2021/077089 2021-01-27 2021-02-20 Magnetometer information assisted mems gyroscope calibration method and calibration system WO2022160391A1 (en)

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
CN202110113501.0 2021-01-27
CN202110113501.0A CN112945271B (en) 2021-01-27 2021-01-27 Magnetometer information-assisted MEMS gyroscope calibration method and system

Publications (1)

Publication Number Publication Date
WO2022160391A1 true WO2022160391A1 (en) 2022-08-04

Family

ID=76238152

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/CN2021/077089 WO2022160391A1 (en) 2021-01-27 2021-02-20 Magnetometer information assisted mems gyroscope calibration method and calibration system

Country Status (2)

Country Link
CN (1) CN112945271B (en)
WO (1) WO2022160391A1 (en)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115406467A (en) * 2022-11-01 2022-11-29 北京开拓航宇导控科技有限公司 Automatic calibration method for MEMS gyroscope
CN115900770A (en) * 2023-02-14 2023-04-04 北京理工大学前沿技术研究院 Online correction method and system for magnetic sensor in airborne environment
CN115979311A (en) * 2023-03-20 2023-04-18 伸瑞科技（北京）有限公司 PIGA (particle image guided Algorithm) cross quadratic term coefficient calibration method, system, equipment and medium

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114526756B (en) * 2022-01-04 2023-06-16 华南理工大学 Unmanned aerial vehicle-mounted multi-sensor correction method, device and storage medium
CN114689901B (en) * 2022-01-27 2023-09-05 北京邮电大学 Accelerometer field calibration method and device

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20110167893A1 (en) * 2008-09-17 2011-07-14 Bae Systems Information And Electronic Systems Integration Inc. Method and apparatus for in-flight calibration of gyroscope using magnetometer reference
US20140202229A1 (en) * 2013-01-23 2014-07-24 Michael E. Stanley Systems and method for gyroscope calibration
US20150285835A1 (en) * 2013-12-23 2015-10-08 InvenSense, Incorporated Systems and methods for sensor calibration
CN107389092A (en) * 2017-06-27 2017-11-24 上海交通大学 A kind of Gyro Calibration method based on Magnetic Sensor auxiliary
CN109696183A (en) * 2019-01-28 2019-04-30 北京华捷艾米科技有限公司 The scaling method and device of Inertial Measurement Unit
CN110177998A (en) * 2017-01-06 2019-08-27 Idhl控股公司 Methods, devices and systems for gyroscope calibration calibration
CN111121824A (en) * 2020-01-03 2020-05-08 西北工业大学 Calibration method of MEMS sensor

Family Cites Families (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP6104169B2 (en) * 2010-11-08 2017-03-29 クアルコム，インコーポレイテッド Device and method for gyro sensor calibration
CN104880201B (en) * 2015-03-26 2016-01-13 武汉大学 MEMS gyro automatic calibration method
CN105547326B (en) * 2015-12-08 2018-04-06 上海交通大学 Gyro and Magnetic Sensor combined calibrating method
CN109631939B (en) * 2018-11-08 2020-07-28 湖北三江航天红峰控制有限公司 Rapid alignment method based on magnetometer and accelerometer

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20110167893A1 (en) * 2008-09-17 2011-07-14 Bae Systems Information And Electronic Systems Integration Inc. Method and apparatus for in-flight calibration of gyroscope using magnetometer reference
US20140202229A1 (en) * 2013-01-23 2014-07-24 Michael E. Stanley Systems and method for gyroscope calibration
US20150285835A1 (en) * 2013-12-23 2015-10-08 InvenSense, Incorporated Systems and methods for sensor calibration
CN110177998A (en) * 2017-01-06 2019-08-27 Idhl控股公司 Methods, devices and systems for gyroscope calibration calibration
CN107389092A (en) * 2017-06-27 2017-11-24 上海交通大学 A kind of Gyro Calibration method based on Magnetic Sensor auxiliary
CN109696183A (en) * 2019-01-28 2019-04-30 北京华捷艾米科技有限公司 The scaling method and device of Inertial Measurement Unit
CN111121824A (en) * 2020-01-03 2020-05-08 西北工业大学 Calibration method of MEMS sensor

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
LI YUPING, QI MIN-HUI;WU LIAN-SHUN;XIANG KUI: "Implementation of an Orientation Estimation Algorithm Based on Complementary Kalman Filter", MEASUREMENT & CONTROL TECHNOLOGY, BEIJING CHANGCHENG HANGKONG CEKONG JISHU YANJIUSUO, CN, 18 March 2017 (2017-03-18), CN , XP055954213, ISSN: 1000-8829, DOI: 10.19708/j.ckjs.2017.03.013 *

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115406467A (en) * 2022-11-01 2022-11-29 北京开拓航宇导控科技有限公司 Automatic calibration method for MEMS gyroscope
CN115406467B (en) * 2022-11-01 2023-03-21 北京开拓航宇导控科技有限公司 Automatic calibration method for MEMS gyroscope
CN115900770A (en) * 2023-02-14 2023-04-04 北京理工大学前沿技术研究院 Online correction method and system for magnetic sensor in airborne environment
CN115979311A (en) * 2023-03-20 2023-04-18 伸瑞科技（北京）有限公司 PIGA (particle image guided Algorithm) cross quadratic term coefficient calibration method, system, equipment and medium

Also Published As

Publication number Publication date
CN112945271B (en) 2022-12-06
CN112945271A (en) 2021-06-11

Similar Documents

Publication Publication Date Title
WO2022160391A1 (en) Magnetometer information assisted mems gyroscope calibration method and calibration system
CN110398257B (en) GPS-assisted SINS system quick-acting base initial alignment method
CN110398245B (en) Indoor pedestrian navigation attitude estimation method based on foot-worn inertial measurement unit
CN110174123B (en) Real-time calibration method for magnetic sensor
CN112562077B (en) Pedestrian indoor positioning method integrating PDR and priori map
CN114485641B (en) Attitude calculation method and device based on inertial navigation device navigation azimuth fusion
CN110567461A (en) Non-cooperative spacecraft attitude and parameter estimation method considering no gyroscope
Li et al. Common frame based unscented quaternion estimator for inertial-integrated navigation
JP2019120587A (en) Positioning system and positioning method
Castellanos et al. A low-cost air data attitude heading reference system for the tourism airplane applications
CN110686684B (en) Optical collaborative orbit determination method for small celestial body surrounding detector
CN116817896A (en) Gesture resolving method based on extended Kalman filtering
CN111649747A (en) IMU-based adaptive EKF attitude measurement improvement method
Peng et al. Design of an embedded icosahedron mechatronics for robust iterative IMU calibration
CN108871319B (en) Attitude calculation method based on earth gravity field and earth magnetic field sequential correction
JP2010096647A (en) Navigation apparatus and estimation method
CN114459478B (en) Inertial measurement unit data fusion method based on attitude kinematic model
CN112683265B (en) MIMU/GPS integrated navigation method based on rapid ISS collective filtering
CN114111770B (en) Horizontal attitude measurement method, system, processing equipment and storage medium
CN114396936B (en) Polynomial optimization-based inertial and magnetic sensor attitude estimation method and system
Takahashi et al. Accelerometer and magnetometer auto-calibration algorithm for attitude determination
CN117128956B (en) Dynamic inclination angle acquisition method based on angular velocity conversion and equipment applying method
CN117433561A (en) Turntable-free MIMU error calibration method based on genetic algorithm
CN116858286A (en) Linear transmission alignment method based on special European style group and earth coordinate system
CN117782081A (en) Method and device for improving gesture resolving precision, electronic equipment and storage medium

Legal Events

Date Code Title Description
121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 21921990

Country of ref document: EP

Kind code of ref document: A1

NENP Non-entry into the national phase

Ref country code: DE

122 Ep: pct application non-entry in european phase

Ref document number: 21921990

Country of ref document: EP

Kind code of ref document: A1