Electronic compass error compensation and solving method, system, medium and device

Abstract

The application provides an electronic compass error compensation and solution method, system, medium and equipment, including: step 1: obtaining electronic compass measurement data according to a three-axis magnetometer and a three-axis accelerometer, obtaining three-axis magnetic field data and three-axis acceleration data; step 2: using an extended Kalman filtering algorithm to optimize the three-axis magnetic field data, obtaining an error compensation matrix; step 3: performing double vector orientation according to the three-axis magnetic field data and the three-axis acceleration data after correction of the error compensation matrix, and obtaining a carrier heading angle. The application introduces multi-sensor constraints into the electronic compass error compensation algorithm, solves the problem that the general compensation algorithm estimates few error parameters, causes fixed deviation after correction, and makes the operation simpler and the measurement precision higher.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of error compensation, in particular to an electronic compass error compensation and solving method, system, medium and equipment. BACKGROUND

[0002] As Figure 1 , the present application discloses a kind of electronic compass error compensation and solving method, system, medium and equipment, which can solve the problems of the prior art.

[0003] Prior art CN108088431A, published on May 29, 2018, discloses a self-correcting electronic compass and its correction method. The correction method includes using multiple magnetometers to collect magnetic field data, but this method still uses traditional methods to optimize and solve the measurement data, with limited precision improvement, and only estimates 9 error parameters, which cannot eliminate misalignment deviation. SUMMARY

[0004] To overcome the defects of the prior art, the present application provides an electronic compass error compensation and solving method, system, medium and equipment.

[0005] The electronic compass error compensation and solving method provided by the present application comprises:

[0006] Step 1: Obtain electronic compass measurement data from three-axis magnetometer and three-axis accelerometer to obtain three-axis magnetic field data and three-axis acceleration data;

[0007] Step 2: Use extended Kalman filter algorithm to optimize and process three-axis magnetic field data to obtain error compensation matrix;

[0008] Step 3: Perform double-vector orientation on three-axis magnetic field data and three-axis acceleration data corrected according to error compensation matrix to obtain carrier heading angle.

[0009] Preferably, before obtaining electronic compass measurement data, the accelerometer and magnetometer of the electronic compass are calibrated respectively;

[0010] For the magnetometer, calibration is performed in a non-magnetic environment manufactured by magnetic shielding barrel, shielding all external magnetic fields, and measuring three-axis zero offset caused by the magnetic field of the magnetometer itself.

[0011] For the accelerometer, under the condition of being stationary on a fixed horizontal platform, the maximum and minimum components of gravitational acceleration along the three axes of the accelerometer are measured, i.e., X. max X min Y max Y min Z max Z min Therefore, the zero offset of the x-axis is calculated. Similarly, Δy and Δz are calculated.

[0012] Preferably, step 2 includes:

[0013] Step 2.1: Model the magnetometer error, the expression is:

[0014]

[0015] in, ε represents the actual measured value of the magnetometer; h represents the ideal measured value of the magnetometer; Q represents a 3*3 matrix containing soft magnetic interference, triaxial non-orthogonality, and scale factor error; b represents zero scale drift and hard magnetic interference error; ε represents zero-mean Gaussian noise.

[0016] Transforming formula (1), we get:

[0017]

[0018] Where ε′ is the transformed zero-mean Gaussian noise; A and B are compensation matrices; a 11 ~a 33 b1 to b3 represent error parameters including soft magnetic interference, triaxial non-orthogonality, and scale factor error; b1 to b3 represent error parameters including zero scale drift and hard magnetic interference error.

[0019] Step 2.2: Obtain the local geomagnetic reference quantity H and its magnitude ||H|| in the geographic coordinate system from the geomagnetic model. The expression for H is:

[0020]

[0021] Among them, H x H y H z These represent the three-axis components of the geomagnetic field vector in the local geographic coordinate system;

[0022] Under ideal conditions free from magnetic interference, the magnetometer measurements satisfy the following:

[0023] ||H|| 2 -h T h=0…………(4)

[0024] According to the coordinate system constraint relationship between the plurality of magnetometers, ideally, the measurement values of two adjacent magnetometers satisfy: wherein, is a coordinate system conversion matrix, and thus:

[0025]

[0026] Step 2.3: error compensation is performed according to formulas (2), (4) and (5) using an extended Kalman filter algorithm, and a system model is constructed;

[0027] System state equation:

[0028] X k =f(X k-1 ,k-1)+W k

[0029] wherein the variable X is:

[0030] X=[a 11 ,a 12 ,a 13 ,a 21 ,a 22 ,a 23 ,a 31 ,a 32 ,a 33 ,b1,b2,b3]

[0031] W k is the system noise at time k, W k ~N(0, Q k ); X k represents the system state quantity; N(0, Q k ) represents a normal distribution with a mean of 0 and a variance of Q k ; Q k represents the variance of the Gaussian distribution;

[0032] System observation equation:

[0033]

[0034] wherein V k is the system observation noise at time k, V k ~N(0, R k ); Z k represents the system observation quantity; h k represents the theoretical three-axis measurement value of the geomagnetic field; R k represents the variance of the Gaussian noise;

[0035] The model also includes:

[0036]

[0037] X k+1 = X k + K k (Z k+1 - h k+1 (X k ))

[0038] P k+1 = (I - K k H k+1 (X k ))P k

[0039] wherein K k represents a filter gain matrix; P k represents a posteriori estimation covariance matrix of the kth process; H k+1 represents an observation matrix, which is represented by a Jacobian matrix obtained by taking partial derivative of h(X k ) in the observation equation; and I represents a unit matrix;

[0040] Step 2.4: After X is iterated for several times by the algorithm, the obtained value contains the magnetic force meter error compensation amount, i.e. the compensation matrix A and B in formula (2) are obtained, so that the correction and compensation of the original measurement data of the magnetic force meter are realized.

[0041] Preferably, the step 3 comprises:

[0042] Step 3.1: Obtain a local gravity acceleration reference vector in a geographic coordinate system, which is expressed as:

[0043]

[0044] The directional cosine matrix is used to represent the orientation relationship between the electronic compass carrier coordinate system b and the geographic coordinate system g, so that the measurement values of the geomagnetic and gravity acceleration in the carrier coordinate system and the reference values of the geomagnetic and gravity acceleration in the geographic coordinate system satisfy the following formula under ideal conditions:

[0045]

[0046] wherein G b represents the actual measurement value of the accelerometer;

[0047] Step 3.2: The geomagnetic field vector and the gravity acceleration vector are not collinear, so the heading is calculated by using the double vector attitude determination principle, and an auxiliary vector is constructed as:

[0048] The geomagnetic field vector is selected as the main vector to be unitized and orthogonalized, and the following formula is obtained:

[0049]

[0050] That is:

[0051] Step 3.3: According to the three-axis rotation attitude transformation matrix of the coordinate system, the following is obtained

[0052]

[0053] Wherein, α, β, represent the carrier attitude angle, respectively, the pitch angle, roll angle, heading angle;

[0054] Step 3.4: According to equation (8), the following is obtained Therefore, according to the corresponding relationship of equations (7) and (8), the heading angle φ can be calculated.

[0055] The electronic compass error compensation and solution system provided by the application comprises:

[0056] Module M1: obtaining electronic compass measurement data according to a three-axis magnetometer and a three-axis accelerometer, obtaining three-axis magnetic field data and three-axis acceleration data;

[0057] Module M2: using an extended Kalman filter algorithm to optimize the three-axis magnetic field data, obtaining an error compensation matrix;

[0058] Module M3: performing double vector orientation according to the three-axis magnetic field data and the three-axis acceleration data corrected by the error compensation matrix, and obtaining the carrier heading angle.

[0059] Preferably, before obtaining the electronic compass measurement data, the accelerometer and the magnetometer of the electronic compass are calibrated respectively;

[0060] For the magnetometer, the calibration is performed in a non-magnetic environment manufactured by a magnetic shielding barrel, shielding all external magnetic fields, and measuring the three-axis zero offset caused by the magnetic field of the magnetometer itself.

[0061] For the accelerometer, under the condition of static state on the fixed water platform, the maximum and minimum components of the gravitational acceleration on the three axes of the accelerometer are measured, that is, X max , X min , Y max , Y min , Z max , Z min Therefore, the x-axis zero offset Δx is calculated Similarly, Δy and Δz are calculated.

[0062] Preferably, the module M2 comprises:

[0063] Module M2.1: modeling the error of the magnetometer, the expression is:

[0064]

[0065] where, is the actual measurement of magnetometer; h is the ideal measurement of magnetometer; Q represents a 3*3 matrix containing soft magnetic interference, three-axis non-orthogonality, scale factor error; b represents zero scale drift, hard magnetic interference error; ε represents zero-mean Gaussian noise;

[0066] Transforming formula (1), we obtain:

[0067]

[0068] where, ε' is the transformed zero-mean Gaussian noise; A, B are compensation matrices; a 11 ~a 33 represent error parameters containing soft magnetic interference, three-axis non-orthogonality, scale factor error; b1~b3 represent error parameters containing zero scale drift, hard magnetic interference error;

[0069] Module M2.2: Obtain the local geomagnetic reference quantity H and its modulus ||H|| in the geographic coordinate system from the geomagnetic model, and the expression of H is:

[0070]

[0071] where, H x , H y , H z respectively represent three-axis components of the geomagnetic field vector in the local geographic coordinate system;

[0072] For an ideal non-magnetic interference environment, the magnetometer measurement value satisfies:

[0073] ||H|| 2 -h T h=0 ……(4)

[0074] According to the coordinate system constraint relationship between multiple magnetometers, the measurement values of adjacent two magnetometers satisfy: where, is the coordinate system conversion matrix, so we have:

[0075]

[0076] Module M2.3: Error compensation is performed according to formulas (2), (4) and (5) using the extended Kalman filtering algorithm, and a system model is constructed;

[0077] System state equation:

[0078] X k =f(X k-1, k-1) + W k

[0079] where variable X is:

[0080] X = [a 11 , a 12 , a 13 , a 21 , a 22 , a 23 , a 31 , a 32 , a 33 , b1, b2, b3]

[0081] W k is the system noise at time k, W k ~ N(0, Q k ); X k represents the system state quantity; N(0, Q k ) represents a normal distribution with mean 0 and variance Q k ; Q k represents the variance of the Gaussian distribution;

[0082] System observation equation:

[0083]

[0084] where V k is the system observation noise at time k, V k ~ N(0, R k ); Z k represents the system observation quantity; h k represents the theoretical three-axis measurement value of the geomagnetic field; R k represents the variance of the Gaussian noise;

[0085] The model also includes:

[0086]

[0087] X k+1 = X k + K k (Z k+1 - h k+1 (X k ))

[0088] P k+1 = (I - K k H k+1 (X k )) P k

[0089] where K k represents the filter gain matrix; P krepresents the posteriori estimation covariance matrix of the kth process; H k+1 represents the observation matrix, which is represented by the Jacobian matrix obtained by taking the partial derivative of h(X k ) in the observation equation; I represents the unit matrix;

[0090] Module M2.4: After X is iterated for several times, the value obtained contains the error compensation amount of the magnetometer, i.e., the compensation matrix A and B in formula (2) are obtained, so as to realize the correction and compensation of the original measurement data of the magnetometer.

[0091] Preferably, the module M3 comprises:

[0092] Module M3.1: Obtain the local gravity acceleration reference vector in the geographic coordinate system, which is expressed as:

[0093]

[0094] The directional cosine matrix is used to represent the orientation relationship between the electronic compass carrier coordinate system b and the geographic coordinate system g, then the measurement values of the geomagnetic and gravity acceleration in the carrier coordinate system and the reference values of the geomagnetic and gravity acceleration in the geographic coordinate system satisfy the following formula under ideal conditions:

[0095]

[0096] wherein, G b represents the actual measurement value of the accelerometer;

[0097] Module M3.2: The geomagnetic field vector and the gravity acceleration vector are not collinear, therefore, the heading is calculated by using the double vector attitude determination principle, and the auxiliary vector is constructed as:

[0098] The geomagnetic field vector is selected as the main vector to be unitized, and the following formula is obtained:

[0099]

[0100] That is:

[0101] Module M3.3: The following formula is calculated according to the three-axis rotation attitude transformation matrix of the coordinate system:

[0102]

[0103] wherein, α, β, represent the carrier attitude angles, which are the pitch angle, the roll angle and the heading angle, respectively;

[0104] Module M3.4: According to formula (8), the following formula is obtained: Therefore, the heading angle φ can be calculated according to the correspondence of (7) and (8).

[0105] The computer readable storage medium storing the computer program is provided according to the application, and the computer program is executed by the processor to realize the steps of the electronic compass error compensation and solution method.

[0106] The electronic device is provided according to the application, and includes a memory, a processor and a computer program stored in the memory and executable on the processor, and the computer program is executed by the processor to realize the steps of the electronic compass error compensation and solution method.

[0107] Compared with the prior art, the application has the following beneficial effects:

[0108] (1) The application introduces the multi-sensor constraint into the electronic compass error compensation algorithm, solves the problem that the general compensation algorithm estimates few error parameters, causes the fixed deviation after correction, makes the operation simpler and the measurement precision higher, and the like.

[0109] (2) The application solves the problems of the traditional solution method, such as complex calculation, large error introduction and the like, by using the double-vector directional principle to solve the heading angle. BRIEF DESCRIPTION OF DRAWINGS

[0110] Other features, objects and advantages of the application will become more apparent from the following detailed description of non-limiting embodiments with reference to the following drawings:

[0111] Figure 1 It is a schematic diagram of the heading measurement and inclination compensation principle of the electronic compass in the prior art;

[0112] Figure 2a , Figure 2b , Figure 2c It is a measurement data distribution effect diagram of the magnetometer under the conditions of no interference, hard magnetic interference and soft and hard magnetic interference, respectively;

[0113] Figure 3 It is a schematic diagram of the constraint relationship between multiple sensors of the used hardware system;

[0114] Figure 4 It is a flowchart of the electronic compass compensation and solution method of the application. DETAILED DESCRIPTION

[0115] The application will be described in detail below with reference to specific embodiments. The following embodiments will help those skilled in the art to further understand the application, but do not limit the application in any form. It should be pointed out that those skilled in the art can make several changes and improvements without departing from the concept of the application. These all belong to the protection scope of the application.

[0116] Embodiment 1:

[0117] As Figure 4 , the application provides an electronic compass error compensation and solution method, comprising the following steps:

[0118] Step 1: Obtain electronic compass measurement data, the electronic compass comprising a plurality of three-axis magnetometers and three-axis accelerometers, to obtain three-axis magnetic field data and three-axis acceleration data;

[0119] Step 2: using an extended Kalman filter algorithm to optimize the three-axis magnetic field data, to obtain an error compensation matrix;

[0120] Step 3: according to the error compensation matrix corrected three-axis magnetic field data and three-axis acceleration data for double vector orientation, to obtain the carrier heading angle.

[0121] There is a fixed constraint relationship between the measurement coordinate systems of the plurality of sensors of the electronic compass, which is converted by an attitude conversion matrix, such as Figure 3 ;

[0122] The step 2 comprises the following sub-steps:

[0123] Step 2.1: model the magnetometer error, the expression being:

[0124]

[0125] wherein, is the actual measurement value of the magnetometer; h is the ideal measurement value of the magnetometer; Q represents a 3*3 matrix containing soft magnetic interference, three-axis non-orthogonality, scale factor error; b represents zero scale drift, hard magnetic interference error; ε represents zero-mean Gaussian noise;

[0126] Transforming formula (1) to obtain:

[0127]

[0128] wherein,

[0129] A, B are compensation matrices; a 11 ~a 33 represent error parameters containing soft magnetic interference, three-axis non-orthogonality, scale factor error; b1~b3 represent error parameters containing zero scale drift, hard magnetic interference error; ε' is the transformed zero-mean Gaussian noise;

[0130] Step 2.2: obtain the local geomagnetic reference quantity H and its modulus ||H|| in the geographic coordinate system from the geomagnetic model, the expression of H being:

[0131]

[0132] H x 、H y 、H z respectively represent the three-axis components of the geomagnetic field vector in the local geographic coordinate system, generally the north component, east component and ground component;

[0133] As Figures 2a-2c , for the ideal non-magnetic interference environment, the magnetometer measurement value should satisfy:

[0134] ||H|| 2 -h T h = 0 …… (4)

[0135] According to the coordinate system constraint relationship between multiple magnetometers, under ideal conditions, the measurement values of adjacent two magnetometers should satisfy: wherein is the coordinate system conversion matrix, therefore:

[0136]

[0137] Step 2.3: error compensation is carried out according to formula (2), (4) and (5) using extended Kalman filtering algorithm, and a system model is constructed;

[0138] System state equation:

[0139] X k = f(X k-1 , k-1) + W k

[0140] wherein the variable X is:

[0141] X = [a 11 , a 12 , a 13 , a 21 , a 22 , a 23 , a 31 , a 32 , a 33 , b1, b2, b3]

[0142] W k is the system noise at k time, W k ~ N(0, Q k ); X k represents the system state quantity; N(0, Q k ) represents a normal distribution with mean 0 and variance Q k ; Q k represents the variance of the Gaussian (normal) distribution;

[0143] System observation equation:

[0144]

[0145] where V k is the system observation noise at time k, V k ~N(0, R k ); Z k represents the system observation; h k represents the theoretical three-axis measurement of the geomagnetic field; R k represents the variance of the Gaussian noise;

[0146] The model also includes:

[0147]

[0148] X k+1 = X k + K k (Z k+1 -h k+1 (X k ))

[0149] P k+1 = (I-K k H k+1 (X k )) P k

[0150] K k represents the filter gain matrix; P k represents the posteriori estimation covariance matrix of the kth process; H k+1 represents the observation matrix, which is represented by the Jacobian matrix obtained by taking the partial derivative of h(X k ) in the observation equation; I represents the unit matrix;

[0151] Step 2.4: After several iterations of the algorithm, the value obtained contains the amount of error compensation for the magnetometer, that is, the compensation matrices A and B in formula (2) are obtained, so as to realize the correction and compensation of the original measurement data of the magnetometer.

[0152] The step 3 includes the following sub-steps:

[0153] Step 3.1: Obtain the local gravity acceleration reference vector in the geographic coordinate system, expressed as:

[0154]

[0155] Use the direction cosine matrix to represent the azimuth relationship between the electronic compass carrier coordinate system b and the geographic coordinate system g, then the measured values of the geomagnetic and gravity acceleration in the carrier coordinate system and the reference values of the geomagnetic and gravity acceleration in the geographic coordinate system should satisfy the following relationship under ideal conditions:

[0156]

[0157] wherein G b represents the actual measured value of the accelerometer;

[0158] Step 3.2: The geomagnetic field vector and the gravity acceleration vector are not collinear, so the heading is calculated by using the double vector attitude determination principle, and the auxiliary vector is constructed as follows:

[0159] The geomagnetic field vector is selected as the main vector for unit orthogonalization, and the following can be obtained:

[0160]

[0161] That is:

[0162] Step 3.3: The is calculated according to the three-axis rotation attitude transformation matrix of the coordinate system

[0163]

[0164] wherein α, β, represent the carrier attitude angles, respectively, the pitch angle, the roll angle, and the heading angle;

[0165] Step 3.4: The heading angle is calculated according to the corresponding relationship of formulas (7) and (8). According to formula (8), the following can be obtained: Therefore, the heading angle φ can be calculated according to the corresponding relationship of formulas (7) and (8);

[0166] Before step 1, it can also include: calibrating the accelerometer and the magnetometer of the electronic compass respectively. For the magnetometer, the calibration is performed in a non-magnetic environment manufactured in a magnetic shielding barrel, which can shield all external magnetic fields, so that the three-axis zero offset caused by the magnetic field of the magnetometer itself can be measured. For the accelerometer, the maximum and minimum components of the gravity acceleration on the three axes of the accelerometer are measured under the condition of static state on the fixed water platform, i.e. max , X min , Y max , Y min , Z max , Z min Therefore, the x-axis zero offset Δx is calculated as follows: Similarly, Δy and Δz are calculated.

[0167] Example 2:

[0168] The application also provides an electronic compass error compensation and solution system, which can be realized by performing the flow steps of the electronic compass error compensation and solution method, i.e., the electronic compass error compensation and solution method can be understood by those skilled in the art as a preferred embodiment of the electronic compass error compensation and solution system.

[0169] The electronic compass error compensation and solution system provided by the application comprises: module M1: obtaining electronic compass measurement data according to a three-axis magnetometer and a three-axis accelerometer to obtain three-axis magnetic field data and three-axis acceleration data; module M2: performing optimization processing on the three-axis magnetic field data using an extended Kalman filtering algorithm to obtain an error compensation matrix; and module M3: performing double-vector orientation according to the three-axis magnetic field data and the three-axis acceleration data corrected according to the error compensation matrix to obtain a carrier heading angle.

[0170] Before obtaining the electronic compass measurement data, the accelerometer and the magnetometer of the electronic compass are calibrated respectively; for the magnetometer, calibration is performed in a non-magnetic environment manufactured in a magnetic shielding barrel, all external magnetic fields are shielded, and three-axis zero offsets caused by the magnetic field of the magnetometer itself are measured; for the accelerometer, the maximum and minimum components of the gravitational acceleration on the three axes of the accelerometer are measured respectively under the condition of stillness on a fixed water platform, i.e., X max , X min , Y max , Y min , Z max , Z min Therefore, the x-axis zero offset Δx is calculated. Similarly, Δy and Δz are calculated.

[0171] The module M2 comprises:

[0172] Module M2.1: modeling the error of the magnetometer, and the expression is:

[0173]

[0174] wherein, is the actual measurement value of the magnetometer; h is the ideal measurement value of the magnetometer; Q represents a 3*3 matrix, which comprises soft magnetic interference, three-axis non-orthogonality, and scale factor error; b represents zero scale drift and hard magnetic interference error; and ε represents zero-mean Gaussian noise;

[0175] The formula (1) is transformed to obtain:

[0176]

[0177] wherein, ε' is the transformed zero-mean Gaussian noise; A and B are compensation matrices; a11 ~a 33 denotes error parameters including soft magnetic interference, three-axis non-orthogonality, scale factor error; b1~b3 denotes error parameters including zero scale drift, hard magnetic interference error;

[0178] Module M2.2: Obtain the local geomagnetic reference quantity H and its modulus ||H|| in the geographic coordinate system from the geomagnetic model, and the expression of H is:

[0179]

[0180] wherein H x , H y , H z respectively represent the three-axis components of the geomagnetic field vector in the local geographic coordinate system;

[0181] For an ideal non-magnetic interference environment, the magnetometer measurement value satisfies:

[0182] ||H|| 2 -h T h=0 ……(4)

[0183] According to the coordinate system constraint relationship between multiple magnetometers, the measurement values of adjacent two magnetometers satisfy: wherein, is the coordinate system conversion matrix, so:

[0184]

[0185] Module M2.3: Error compensation is performed according to formulas (2), (4) and (5) using an extended Kalman filtering algorithm, and a system model is constructed;

[0186] System state equation:

[0187] X k =f(X k-1 , k-1)+W k

[0188] wherein the variable X is:

[0189] X=[a 11 , a 12 , a 13 , a 21 , a 22 , a 23 , a 31 , a 32 , a 33 , b1, b2, b3]

[0190] W k is the system noise at time k, W k~ N(0, Q k ) ; X k represents the system state quantity; N(0, Q k ) represents a normal distribution with a mean of 0 and a variance of Q k Q k represents the variance of the Gaussian distribution;

[0191] System observation equation:

[0192]

[0193] wherein V k is the system observation noise at time k, V k ~ N(0, R k ) ; Z k represents the system observation quantity; h k represents the theoretical three-axis measurement value of the geomagnetic field; R k represents the variance of the Gaussian noise;

[0194] The model further comprises:

[0195]

[0196] X k+1 = X k + K k (Z k+1 -h k+1 (X k ))

[0197] P k+1 = (I-K k H k+1 (X k )) P k

[0198] wherein K k represents the filter gain matrix; Pk represents the posteriori estimation covariance matrix of the kth process; H k+1 represents the observation matrix, which is represented by the Jacobian matrix obtained by taking the partial derivative of h(X k ) in the observation equation; I represents the unit matrix;

[0199] Module M2.4: After several iterations of the algorithm, the value obtained contains the error compensation quantity of the magnetometer, that is, the compensation matrix A and B in formula (2) are obtained, so that the correction and compensation of the original measurement data of the magnetometer are realized.

[0200] The module M3 comprises:

[0201] Module M3.1: Obtain the local gravity acceleration reference vector in the geographic coordinate system, which is expressed as:

[0202]

[0203] with the direction cosine matrix to represent the azimuth relationship between the electronic compass carrier coordinate system b and the geographic coordinate system g, then the measured values of the geomagnetic and gravity acceleration in the carrier coordinate system and the reference values of the geomagnetic and gravity acceleration in the geographic coordinate system satisfy the following relationship under ideal conditions:

[0204]

[0205] wherein G b represents the actual measured value of the accelerometer;

[0206] Module M3.2: The geomagnetic field vector and the gravity acceleration vector are not collinear, so the heading is calculated by using the double vector attitude determination principle, and the auxiliary vector is constructed as:

[0207] The geomagnetic field vector is selected as the main vector for unit orthogonalization, and the following is obtained:

[0208]

[0209] That is:

[0210] Module M3.3: The following is calculated according to the three-axis rotation attitude transformation matrix of the coordinate system:

[0211]

[0212] wherein α, β, represent the carrier attitude angles, respectively the pitch angle, the roll angle, and the heading angle;

[0213] Module M3.4: The following is obtained according to equation (8): Therefore, the heading angle φ can be calculated according to the corresponding relationship of equations (7) and (8).

[0214] Those skilled in the art know that, in addition to implementing the system, device and each module thereof provided by the present application in the form of pure computer readable program code, the same program can also be realized by logically programming the method steps in the form of logic gates, switches, application specific integrated circuits, programmable logic controllers and embedded microcontrollers. Therefore, the system, device and each module thereof provided by the present application can be considered as a hardware component, and the modules included therein for realizing various programs can also be considered as structures within the hardware component; the modules for realizing various functions can also be considered as both software programs for realizing methods and structures within hardware components.

[0215] The specific embodiments of the present application are described above. It needs to be understood that the present application is not limited to the specific embodiments described above, and various changes or modifications can be made by those skilled in the art within the scope of the claims, which does not affect the essential content of the present application. The embodiments of the present application and the features in the embodiments can be combined with each other at will without conflict.

Claims

1. An electronic compass error compensation and resolution method, characterized in that, Comprise: Step 1: obtain electronic compass measurement data according to three-axis magnetometer and three-axis accelerometer, obtain three-axis magnetic field data and three-axis acceleration data; Step 2: the three-axis magnetic field data is processed by using extended Kalman filter algorithm, and error compensation matrix is obtained; Step 3: according to the three-axis magnetic field data corrected by error compensation matrix and three-axis acceleration data, double vector orientation is carried out, and the carrier heading angle is solved; The step 2 comprises: Step 2.1: the magnetometer error is modeled, and the expression is: wherein, is the magnetometer actual measurement; h is the magnetometer ideal measurement; Q represents a 3*3 matrix containing soft magnetic disturbances, three-axis misalignment, scale factor errors; b represents zero scale drift, hard magnetic disturbance errors; ε represents zero-mean Gaussian noise; The formula (1) is transformed to obtain: wherein ε' is the converted zero-mean Gaussian noise; A, B are compensation matrices; a 11 ~a 33 denote error parameters including soft magnetic interference, three-axis non-orthogonality, scale factor error; b1~b3 denote error parameters including zero scale drift, hard magnetic interference error. Step 2.2: obtain the local geomagnetic reference quantity H and its modulus ‖H‖ in the geographic coordinate system from the geomagnetic model, and the expression of H is: where H x , H y , H z denote the three-axis components of the geomagnetic field vector in the local geographic coordinate system, respectively. For ideal non-magnetic interference environment, the magnetometer measurement value satisfies: ‖H‖ 2 -h T h = 0 (4) According to the coordinate system constraint relationship between the plurality of magnetometers, ideally, the measurement values of the two adjacent magnetometers satisfy: wherein, is a coordinate system conversion matrix, and thus has: Step 2.3: according to formula (2), (4) and (5), the error compensation is carried out by using extended Kalman filter algorithm, and the system model is constructed; System state equation: X k = f(X k-1 , k - 1) + W k Wherein the variable X is: X = [a 11 ,a 12 ,a 13 ,a 21 ,a 22 ,a 23 ,a 31 ,a 32 ,a 33 ,b1,b2,b3] W k is the system noise at time k, W k ~ N(0, Q k ); X k represents the system state quantity; N(0, Q k ) represents a normal distribution with mean 0 and variance Q k ; Q k represents the variance of the Gaussian distribution; System observation equation: where V k is the system observation noise at time k, V k ~ N(0, R k ); Z k represents the system observation; h k represents the theoretical three-axis measurement of the geomagnetic field; and R k represents the variance of the Gaussian noise. The model also includes: X k+1 = X k + K k (Z k+1 - h k+1 (X k )) P k+1 = (I - K k H k+1 (X k ))P k where K k represents a filter gain matrix; P k represents a posteriori estimation covariance matrix of the kth process; H k+1 represents an observation matrix, which is represented by a Jacobian matrix obtained by taking partial derivatives of h(X k ) in the observation equation; and I represents an identity matrix. Step 2.4: after X is iterated for several times by the algorithm, the obtained value contains the error compensation quantity of the magnetometer, that is, the compensation matrix A and B in formula (2) are obtained, so that the correction and compensation of the original measurement data of the magnetometer are realized; The step 3 comprises: Step 3.1: obtain the local gravity acceleration reference vector in the geographic coordinate system, and the expression is: Direction cosine matrix The orientation between the body coordinate system b and the geographic coordinate system g is represented by a direction cosine matrix Cbg. The measurements of the geomagnetic and gravitational acceleration in the body coordinate system and the reference values of the geomagnetic and gravitational acceleration in the geographic coordinate system satisfy the following equations under ideal conditions: G b represents the actual measured value of the accelerometer; Step 3.2: The geomagnetic field vector and the gravity acceleration vector are not collinear, so the heading is calculated using the double vector attitude determination principle, and the auxiliary vector is constructed as: The geomagnetic field vector is selected as the main vector to be unitized, and the expression is: That is, Step 3.3: Calculate the transformation matrix according to the coordinate system three-axis rotation pose transformation matrix wherein α, β, represent the carrier attitude angles, respectively, the pitch angle, the roll angle, and the heading angle; Step 3.4: According to (8) Thus, the heading angle φ can be calculated according to the correspondence of (7) and (8).

2. The electronic compass error compensation and resolution method of claim 1, wherein, Before obtaining the electronic compass measurement data, the accelerometer and the magnetometer of the electronic compass are calibrated respectively; For the magnetometer, the non-magnetic environment is manufactured in the magnetic shielding barrel, all external magnetic fields are shielded, and the three-axis zero offset caused by the magnetic field of the magnetometer itself is measured; For the accelerometer, the maximum and minimum components of the gravitational acceleration in the three axes of the accelerometer, i.e. X max min max min max min Thus, the x-axis zero offset is calculated as Similarly, Δy, Δz are calculated.​​​​​ 3. An electronic compass error compensation and resolution system, characterized by, Comprise: Module M1: obtain electronic compass measurement data according to three-axis magnetometer and three-axis accelerometer, obtain three-axis magnetic field data and three-axis acceleration data; Module M2: the three-axis magnetic field data is processed by using extended Kalman filter algorithm, and error compensation matrix is obtained; Module M3: according to the three-axis magnetic field data corrected by error compensation matrix and three-axis acceleration data, double vector orientation is carried out, and the carrier heading angle is solved; The module M2 comprises: Module M2.1: the magnetometer error is modeled, and the expression is: wherein, is the magnetometer actual measurement; h is the magnetometer ideal measurement; Q represents a 3*3 matrix containing soft magnetic disturbances, three-axis misalignment, scale factor errors; b represents zero scale drift, hard magnetic disturbance errors; ε represents zero-mean Gaussian noise; The formula (1) is transformed to obtain: wherein ε' is the converted zero-mean Gaussian noise; A, B are compensation matrices; a 11 ~a 33 denote error parameters including soft magnetic interference, three-axis non-orthogonality, scale factor error; b1~b3 denote error parameters including zero scale drift, hard magnetic interference error. Module M2.2: obtain the local geomagnetic reference quantity H and its modulus ‖H‖ in the geographic coordinate system from the geomagnetic model, and the expression of H is: where H x , H y , H z respectively represent the three-axis components of the geomagnetic field vector in the local geographic coordinate system; For ideal non-magnetic interference environment, the magnetometer measurement value satisfies: ‖H‖ 2 -h T h=0 ………… (4) According to the coordinate system constraint relationship between the plurality of magnetometers, ideally, the measurement values of the two adjacent magnetometers satisfy: wherein, is a coordinate system conversion matrix, and thus has: Module M2.3: according to formula (2), (4) and (5), the error compensation is carried out by using extended Kalman filter algorithm, and the system model is constructed; System state equation: X k = f(X k-1 ,k-1) + W k Wherein the variable X is: X = [a 11 ,a 12 ,a 13 ,a 21 ,a 22 ,a 23 ,a 31 ,a 32 ,a 33 ,b1,b2,b3] W k is the system noise at time k, W k ~ N(0, Q k ); X k represents the system state quantity; N(0, Q k ) represents a normal distribution with mean 0 and variance Q k ; Q k represents the variance of the Gaussian distribution; System observation equation: where V k is the system observation noise at time k, V k ~ N(0, R k ); Z k represents the system observation; h k represents the theoretical three-axis measurement of the geomagnetic field; and R k represents the variance of the Gaussian noise. The model also includes: X k+1 = X k + K k (Z k+1 - h k+1 (X k )) P k+1 = (I - K k H k+1 (X k ))P k where K k represents a filter gain matrix; P k represents a posteriori estimation covariance matrix of the kth process; H k+1 represents an observation matrix, which is represented by a Jacobian matrix obtained by taking partial derivatives of h(X k ) in the observation equation; and I represents an identity matrix. Module M2.4: after X is iterated for several times by the algorithm, the obtained value contains the error compensation quantity of the magnetometer, that is, the compensation matrix A and B in formula (2) are obtained, so that the correction and compensation of the original measurement data of the magnetometer are realized; The module M3 comprises: Module M3.1: obtain the local gravity acceleration reference vector in the geographic coordinate system, and the expression is: Direction cosine matrix The orientation between the body coordinate system b and the geographic coordinate system g is represented by a direction cosine matrix Cbg. The measurements of the geomagnetic and gravitational acceleration in the body coordinate system and the reference values of the geomagnetic and gravitational acceleration in the geographic coordinate system satisfy the following equations under ideal conditions: G b represents the actual measured value of the accelerometer; Module M3.2: The geomagnetic field vector and the gravity acceleration vector are not collinear, therefore, the heading solution is performed using the double vector attitude determination principle, and the auxiliary vector is constructed as: The geomagnetic field vector is selected as the main vector to be unitarily orthogonalized, and the following is obtained: That is, Module M3.3: Computing the three-axis rotation pose transformation matrix from the coordinate system wherein α, β, represent the carrier attitude angles, respectively, the pitch angle, the roll angle, and the yaw angle; Module M3.4: φ = arctan (8) Thus, the heading angle φ can be calculated according to the correspondence of (7) and (8).

4. The electronic compass error compensation and resolution system of claim 3, wherein, Before obtaining the measurement data of the electronic compass, the accelerometer and the magnetometer of the electronic compass are calibrated respectively; For the magnetometer, the calibration is performed in a non-magnetic environment manufactured in a magnetic shielding barrel, all external magnetic fields are shielded, and the three-axis zero offset caused by the magnetic field of the magnetometer itself is measured; For the accelerometer, the maximum and minimum components of the gravitational acceleration in the three axes of the accelerometer, i.e. X max min max min max min Thus, the x-axis zero offset is calculated as Similarly, Δy, Δz are calculated.​​​​​ 5. A computer readable storage medium storing a computer program, characterized in that, The computer program is executed by the processor to realize the steps of the electronic compass error compensation and solving method in claim 1 or 2.

6. An electronic device comprising a memory, a processor, and a computer program stored on the memory and executable on the processor, characterized in that, The computer program is executed by the processor to realize the steps of the electronic compass error compensation and solving method in claim 1 or 2.

Images