System and method for real-time magnetic field matching

Abstract

The present invention provides a real-time magnetic field matching system and method, the system comprising a sensor acquisition module (1), a magnetic field signal and heading angle storage sequence update module (2), a magnetic field matching module (3), a heading angle correction output module (4) and an error updating module (5). The sensor acquisition module (1) is used to perform sensor data acquisition, the magnetic field signal and heading angle storage sequence update module (2) is used to update the magnetic field signal storage sequence and the heading angle storage sequence, the magnetic field matching module (3) is used to perform magnetic field matching, the heading angle correction output module (4) is used to output data after heading angle correction, and the error updating module (5) is used to update the heading angle storage sequence according to the angle error.

Description

Technical Field

[0001] The present invention relates to the technical field of multi-sensor information fusion, and mainly to magnetic field matching and calibration technology. Background Art

[0002] The MEMS attitude system outputs pitch, roll and heading signals, and is widely used in the field of industrial control due to its low cost, low power consumption and small size. The pitch and roll angles of the MEMS attitude system can be stabilized after being corrected by the accelerometer signal, but the heading angle drifts over time, which will produce a large cumulative error.

[0003] The three-axis magnetometer measures the magnetic field components in three directions of the carrier and has the advantage of being resistant to shock and vibration. The magnetometer collects magnetic field signals in real time to form a measurement sequence. According to the matching characteristic quantities of the magnetic field signal such as magnetic field intensity, horizontal component, vertical component, magnetic field angle, and magnetic field gradient, the error correction of the MEMS attitude system can be completed. Summary of the invention

[0004] The present invention adopts the information fusion technology of multiple sensors, uses the magnetic field signal storage sequence of a three-axis magnetometer to perform feature point matching, and uses the angular velocity signal of a MEMS attitude system as a constraint condition, thereby providing a real-time magnetic field matching method to complete the heading angle update of the MEMS attitude system and eliminate its cumulative error.

[0005] The present invention provides a real-time magnetic field matching system and method, the system comprising a sensor acquisition module (1), a magnetic field signal and heading angle storage sequence update module (2), a magnetic field matching module (3), a heading angle correction output module (4) and an error updating module (5). The sensor acquisition module (1) is used to perform sensor data acquisition, the magnetic field signal and heading angle storage sequence update module (2) is used to update the magnetic field signal storage sequence and the heading angle storage sequence, the magnetic field matching module (3) is used to perform magnetic field matching, the heading angle correction output module (4) is used to output data after heading angle correction, and the error updating module (5) is used to update the heading angle storage sequence according to the angle error. BRIEF DESCRIPTION OF THE DRAWINGS

[0006] Figure 1 System and process diagram of real-time magnetic field matching Implementation

[0007] The present invention utilizes the magnetic field signal collected by the three-axis magnetometer, the angular velocity signal provided by the gyroscope of the MEMS attitude system, and the heading angle signal of the MEMS attitude system to perform comprehensive information processing.

[0008] The present invention updates the magnetic field signal storage sequence and the heading angle storage sequence in real time, uses the currently collected sensor information and the storage sequence to perform feature point matching and correlation processing, and after finding the best matching point, corrects the heading angle of the MEMS attitude system and outputs it, and updates the value in the heading angle storage sequence.

[0009] refer to Figure 1 , showing a system and method for real-time magnetic field matching. The system comprises a sensor acquisition module (1), a magnetic field signal and heading angle storage sequence update module (2), a magnetic field matching module (3), a heading angle correction output module (4) and an error update module (5). The sensor acquisition module (1) is used to acquire sensor data, the magnetic field signal and heading angle storage sequence update module (2) is used to update the magnetic field signal storage sequence and the heading angle storage sequence, the magnetic field matching module (3) is used to perform magnetic field matching, the heading angle correction output module (4) is used to output data after heading angle correction, and the error update module (5) is used to update the heading angle storage sequence according to the angle error. The specific implementation process is as follows:

[0010] 1. Data collection from sensors

[0011] The heading angle D and angular velocity vector W of the MEMS attitude system are collected every sampling period T, where the three components of the angular velocity vector W are Wx, Wy, and Wz, and the magnetic field vector M of the three-axis magnetometer, where the three components of the magnetic field vector M are Mx, My, and Mz. The update period of the magnetic field signal storage sequence and the heading angle storage sequence is T0=N0*T, and N0 is preferably 100.

[0012] 2. Update the magnetic field signal storage sequence and heading angle storage sequence

[0013] The magnetic field signal storage sequence is M[i] (i=0, 1…N-1) with a total of N storage points, and N is preferably 400. The heading angle storage sequence D[j] (j=0, 1…N-1, N=400) is also N storage points. Let the storage sequence counter variable FC (0≤FC≤N-1) count from zero.

[0014] The magnetic field signal storage sequence and heading angle storage sequence are updated at a period of T0. The processing steps are as follows:

[0015] Step 1: At the beginning of power-on, the storage sequence counter variable FC=0, assign the four magnetic field vector sampling points obtained by continuous acquisition to M[0], M[1], M[2], and M[3]; assign the four heading angle sampling points obtained by continuous acquisition to D[0], D[1], D[2], and D[3]; and increase the value of the counter variable FC by 1. Each storage point M[i] is a three-dimensional array.

[0016] Step 2: The current storage sequence counter variable FC, the magnetic field vector M, heading angle D and angular velocity vector W are collected at the current moment, and the normalized vector of the current magnetic field vector sampling point is dot-producted with the normalized vector of the nearest non-zero storage point M[4*FC-1] in the magnetic field signal storage sequence:

[0017]

[0018] In the formula, ‖M‖ represents the modulus operation, that is, That is, the information of magnetic field strength is utilized.

[0019] Step 3: If Γ<0.9, assign the current moment and the three previous consecutive magnetic field vector sampling points to M[4*FC], M[4*FC+1], M[4*FC+2], M[4*FC+3], assign the current moment and the three previous consecutive heading angle sampling points to D[4*FC], D[4*FC+1], D[4*FC+2], D[4*FC+3], and go to step 4;

[0020] Step 4: Increase the value of the storage sequence counter variable FC by 1. Continue to execute step 2. When FC=100, set FC=1, that is, restart counting from 1. Execute the next data update.

[0021] 3. Magnetic field matching

[0022] When performing magnetic field matching of feature points, the data sampling period of the angular velocity, heading angle and three-axis magnetometer of the MEMS attitude system is T, that is, data is collected every time T. The magnetic field intensity error threshold E0 is set, and E0 is preferably 0.5 milligauss, the normalized vector dot product judgment threshold E1 is set, and E1 is preferably 0.99, the heading angle error threshold E2 is set, and E2 is preferably 2.0 degrees, and the angular velocity judgment threshold E3 is set, and E3 is preferably 5 degrees per second.

[0023] Collect data in real time, and obtain the magnetic field vector M, heading angle D and angular velocity vector W at the current moment. The specific steps for feature point magnetic field matching are as follows:

[0024] Step S1: compare the current magnetic field vector M with the stored points in the magnetic field signal storage sequence one by one and perform normalized dot product operations respectively, and compare and calculate the current heading angle D with the stored points in the heading angle storage sequence:

[0025] εm[i]=ABS(‖M‖-‖M[i]‖)

[0026]

[0027] εd[i]=ABS(DD[i])

[0028] Where i=0,1…N-1, ABS represents absolute value operation.

[0029] If the storage point currently being searched meets the following six conditions:

[0030] Condition 1: εm[i] <E0;

[0031] Condition 2: Γ[i]>E1;

[0032] Condition 3: εd[i] <E2;

[0033] Condition 4: Wx <E3;

[0034] Condition 5: Wy <E3;

[0035] Condition 6: Wz <E3;

[0036] The storage point currently being searched is considered to be preliminarily matched and the process goes to step S2. If the preliminarily matched points in the magnetic field signal storage sequence are not qualified, the magnetic field matching is terminated.

[0037] Step S2: If the i-th point in the current searched storage sequence is preliminarily matched, the three consecutive magnetic field vector sampling points and three heading angle sampling points before the current moment are compared and dot-producted with the magnetic field signal storage sequence and the three storage points before the i-th point in the heading angle storage sequence according to the method in step S1. If all conditions 1, 2, and 3 in step S1 are met, it is considered that the magnetic field match is successful and go to step S3; otherwise, go to step S1 to continue searching until the entire storage sequence is searched.

[0038] Step 3: The magnetic field match of the i-th point is successfully found, and the corresponding value D[i] in the heading angle storage sequence is used as the optimal value Dop, and the angle error ε=Dop-D.

[0039] 4. Output after heading angle correction

[0040] After the real-time magnetic field matching is successful, it is considered that the searched optimal value Dop is closer to the actual heading angle of the system, and D=Dop is set as the heading angle output signal of the MEMS attitude system at the current moment.

[0041] 5. Update the heading angle storage sequence according to the angle error

[0042] According to the angle error ε calculated by the optimal value Dop searched by magnetic field matching, the value in the heading angle storage sequence is updated, and D[i]+ε(i=0,1…N-1) is used as the latest heading angle storage sequence for subsequent use.

[0043] The embodiment of the present invention further provides a computer-readable storage medium, on which a computer program is stored, and when the computer program is executed by a processor, each process of the above-mentioned signal control method embodiment is implemented, and the same technical effect can be achieved. To avoid repetition, it is not repeated here. The computer-readable storage medium is, for example, a read-only memory (ROM), a random access memory (RAM), a magnetic disk or an optical disk.

[0044] It should be noted that, in this article, the terms "include", "comprises" or any other variations thereof are intended to cover non-exclusive inclusion, so that a process, method, article or device including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, article or device. In the absence of further restrictions, an element defined by the sentence "comprises a ..." does not exclude the existence of other identical elements in the process, method, article or device including the element.

[0045] Through the description of the above implementation methods, those skilled in the art can clearly understand that the above-mentioned embodiment methods can be implemented by means of software plus a necessary general hardware platform, and of course can also be implemented by hardware, but in many cases the former is a better implementation method. Based on such an understanding, the technical solution of the present invention is essentially or the part that contributes to the prior art can be embodied in the form of a software product, which is stored in a storage medium (such as ROM / RAM, magnetic disk, optical disk), including a number of instructions for a terminal to execute the methods described in each embodiment of the present invention.

Claims

1. A real-time magnetic field matching system, comprising a sensor acquisition module (1), a magnetic field signal and heading angle storage sequence update module (2), a magnetic field matching module (3), a heading angle correction output module (4) and an error update module (5), wherein the sensor acquisition module is used to acquire sensor data, the magnetic field signal and heading angle storage sequence update module is used to update the magnetic field signal storage sequence and the heading angle storage sequence, the magnetic field matching module is used to perform magnetic field matching, the heading angle correction output module is used to output data after heading angle correction, and the error update module is used to update the heading angle storage sequence according to the angle error; The sensor acquisition module performs sensor data acquisition, including: The heading angle D and angular velocity vector W of the MEMS attitude system are collected every sampling period T, where the three components of the angular velocity vector W are Wx, Wy, and Wz, and the magnetic field vector M of the three-axis magnetometer, where the three components of the magnetic field vector M are Mx, My, and Mz, respectively. The update period of the magnetic field signal storage sequence and the heading angle storage sequence is T0=N0*T; The magnetic field signal and heading angle storage sequence update module is used to update the magnetic field signal storage sequence and the heading angle storage sequence, specifically including: the magnetic field signal storage sequence is M[i], i=0,1...N-1, with a total of N storage points, and the heading angle storage sequence is D[j], j=0,1...N-1, also with N storage points. Set the storage sequence counter variable FC, 0≤FC≤N-1, counting from zero, and the magnetic field signal storage sequence and the heading angle storage sequence update cycle is T0. The processing steps are as follows: Step 1: At the beginning of power-on, the storage sequence counter variable FC=0, and the four magnetic field vector sampling points obtained by continuous acquisition are assigned to M[0], M[1], M[2], and M[3]; the four heading angle sampling points obtained by continuous acquisition are assigned to D[0], D[1], D[2], and D[3]; the value of the counter variable FC increases by 1, and each storage point M[i] is a three-dimensional array; Step 2: The current storage sequence counter variable FC, the magnetic field vector M, heading angle D and angular velocity vector W are collected at the current moment, and the normalized vector of the current magnetic field vector sampling point is dot-producted with the normalized vector of the nearest non-zero storage point M[4*FC-1] in the magnetic field signal storage sequence: In the formula, ‖M‖ represents the modulus operation, that is, That is, the information of magnetic field strength is utilized; Step 3: If Γ<0.9, assign the current moment and the three previous consecutive magnetic field vector sampling points to M[4*FC], M[4*FC+1], M[4*FC+2], M[4*FC+3], assign the current moment and the three previous consecutive heading angle sampling points to D[4*FC], D[4*FC+1], D[4*FC+2], D[4*FC+3], and go to step 4; Step 4: The value of the storage sequence counter variable FC is increased by 1, and step 2 is continued. When FC=N-1, FC=1 is set, that is, counting starts again from 1, and the next data update is performed; The magnetic field matching module is used for magnetic field matching, specifically including: when performing magnetic field matching of feature points, the data sampling period of the angular velocity, heading angle and three-axis magnetometer of the MEMS attitude system is T, that is, data is collected once every time T, and the magnetic field intensity error threshold E0, the normalized vector dot product judgment threshold E1, the heading angle error threshold E2, and the angular velocity judgment threshold E3 are set, and data is collected in real time. At the current moment, the magnetic field vector M, the heading angle D and the angular velocity vector W are collected. The specific steps for magnetic field matching of feature points are as follows: Step S1: compare the current magnetic field vector M with the stored points in the magnetic field signal storage sequence one by one and perform normalized dot product operations respectively, and compare and calculate the current heading angle D with the stored points in the heading angle storage sequence: εm[i]=ABS(‖M‖-‖M[i]‖) εd[i]=ABS(DD[i]) Where i = 0, 1…N-1, ABS means absolute value operation; If the storage point currently being searched meets the following six conditions: Condition 1: εm[i] <E0; Condition 2: Γ[i]>E1; Condition 3: εd[i] <E2; Condition 4: Wx <E3; Condition 5: Wy <E3; Condition 6: Wz <E3; It is considered that the storage point currently being searched has qualified preliminary matching, and the process goes to step S2. If the preliminary matching with all the storage points in the magnetic field signal storage sequence fails, the magnetic field matching is terminated. Step S2: If the i-th point in the current searched storage sequence is preliminarily matched, the three consecutive magnetic field vector sampling points and the three heading angle sampling points before the current moment are compared and dot-producted with the magnetic field signal storage sequence and the three storage points before the i-th point in the heading angle storage sequence according to the method in step S1. If all of the conditions 1, 2, and 3 in step S1 are met, it is considered that the magnetic field match is successful, and the process goes to step S3; otherwise, the process goes to step S1 to continue searching until the entire storage sequence is searched; Step S3: The magnetic field match of the i-th point is successfully found, and the corresponding value D[i] in the heading angle storage sequence is used as the optimal value Dop, and the angle error ε=Dop-D.

2. The real-time magnetic field matching system according to claim 1, wherein the heading angle correction output module is used to output data after heading angle correction, specifically comprising: After the real-time magnetic field matching is successful, it is considered that the searched optimal value Dop is closer to the actual heading angle of the system, and D=Dop is set as the heading angle output signal of the MEMS attitude system at the current moment.

3. The real-time magnetic field matching system according to claim 2, wherein the error updating module is used to update the heading angle storage sequence according to the angle error, specifically comprising: According to the angle error ε calculated by the optimal value Dop searched by the magnetic field matching, the value in the heading angle storage sequence is updated, and D[i]+ε, i=0,1…N-1 is used as the latest heading angle storage sequence.

4. A real-time magnetic field matching method, comprising the following steps: (1) Sensor data collection, specifically: The heading angle D and angular velocity vector W of the MEMS attitude system are collected every sampling period T, where the three components of the angular velocity vector W are Wx, Wy, and Wz, and the magnetic field vector M of the three-axis magnetometer, where the three components of the magnetic field vector M are Mx, My, and Mz, respectively. The update period of the magnetic field signal storage sequence and the heading angle storage sequence is T0=N0*T; (2) The magnetic field signal storage sequence and heading angle storage sequence are updated, specifically: The magnetic field signal storage sequence is M[i], i=0,1…N-1, with a total of N storage points. The heading angle storage sequence is D[j], j=0,1…N-1, also with N storage points. Suppose the storage sequence counter variable FC, 0≤FC≤N-1, starts counting from zero. The update cycle of the magnetic field signal storage sequence and the heading angle storage sequence is T0. The processing steps are as follows: Step 1: At the beginning of power-on, the storage sequence counter variable FC=0, and the four magnetic field vector sampling points obtained by continuous acquisition are assigned to M[0], M[1], M[2], and M[3]; the four heading angle sampling points obtained by continuous acquisition are assigned to D[0], D[1], D[2], and D[3]; the value of the counter variable FC increases by 1, and each storage point M[i] is a three-dimensional array; Step 2: The current storage sequence counter variable FC, the magnetic field vector M, heading angle D and angular velocity vector W are collected at the current moment, and the normalized vector of the current magnetic field vector sampling point is dot-producted with the normalized vector of the nearest non-zero storage point M[4*FC-1] in the magnetic field signal storage sequence: In the formula, ‖M‖ represents the modulus operation, that is, That is, the information of magnetic field strength is utilized; Step 3: If Γ<0.9, assign the current moment and the three previous consecutive magnetic field vector sampling points to M[4*FC], M[4*FC+1], M[4*FC+2], M[4*FC+3], assign the current moment and the three previous consecutive heading angle sampling points to D[4*FC], D[4*FC+1], D[4*FC+2], D[4*FC+3], and go to step 4; Step 4: The value of the storage sequence counter variable FC is increased by 1, and step 2 is continued. When FC=N-1, FC=1 is set, that is, counting starts again from 1, and the next data update is performed; (3) Magnetic field matching, specifically: When performing magnetic field matching of feature points, the data sampling period of the angular velocity, heading angle and three-axis magnetometer of the MEMS attitude system is T, that is, data is collected every time T, and the magnetic field intensity error threshold E0, normalized vector dot product judgment threshold E1, heading angle error threshold E2, angular velocity judgment threshold E3 are set, and data is collected in real time. At the current moment, the magnetic field vector M, heading angle D and angular velocity vector W are collected. The specific steps for performing magnetic field matching of feature points are as follows: Step S1: compare the current magnetic field vector M with the stored points in the magnetic field signal storage sequence one by one and perform normalized dot product operations respectively, and compare and calculate the current heading angle D with the stored points in the heading angle storage sequence: εm[i]=ABS(‖M‖-‖M[i]‖) εd[i]=ABS(DD[i]) Where i = 0, 1...N-1, ABS means absolute value operation, If the storage point currently being searched meets the following six conditions: Condition 1: εm[i] <E0; Condition 2: Γ[i]>E1; Condition 3: εd[i] <E2; Condition 4: Wx <E3; Condition 5: Wy <E3; Condition 6: Wz <E3; It is considered that the storage point currently being searched has qualified preliminary matching, and the process goes to step S2. If the preliminary matching with all the storage points in the magnetic field signal storage sequence fails, the magnetic field matching is terminated. Step S2: If the i-th point in the current searched storage sequence is preliminarily matched, the three consecutive magnetic field vector sampling points and the three heading angle sampling points before the current moment are compared and dot-producted with the magnetic field signal storage sequence and the three storage points before the i-th point in the heading angle storage sequence according to the method in step S1. If all of them meet conditions 1, 2, and 3 in step S1, it is considered that the magnetic field match is successful, and go to step S3; Otherwise, go to step S1 and continue searching until the entire storage sequence is searched; Step S3: The magnetic field match of the i-th point is successfully found, and the corresponding value D[i] in the heading angle storage sequence is used as the optimal value Dop, and the angle error ε=Dop-D; (4) Output after heading angle correction, specifically: After the real-time magnetic field matching is successful, it is considered that the searched optimal value Dop is closer to the actual heading angle of the system, and D=Dop is set as the heading angle output signal of the MEMS attitude system at the current moment; (5) Update the heading angle storage sequence according to the angle error, specifically: According to the angle error ε calculated by the optimal value Dop searched by the magnetic field matching, the value in the heading angle storage sequence is updated, and D[i]+ε, i=0,1…N-1 is used as the latest heading angle storage sequence.

5. A computer-readable storage medium storing a computer program, wherein the computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, the method according to claim 4 is implemented.

Images