A MEMS IMU initial alignment method based on nonlinear optimization of magnetic heading

Abstract

The present invention discloses a MEMS IMU initial alignment method based on nonlinear optimized magnetic heading, comprising the following steps: S1, determining the true magnetic vector and magnetic declination of the current position through a world geomagnetic model; S2, calculating the initial magnetic heading, pitch angle, and roll angle through the MEMS IMU; S3, constructing a nonlinear optimized magnetic heading model; S4, solving the nonlinear optimized magnetic heading model to obtain an optimal rotation matrix calculated based on an optimal rotation matrix increment, thereby converting the optimal initial alignment heading angle ψ b es t; This initial alignment method solves the problem of insufficient alignment accuracy of the existing MEMS IMU initial alignment method based on magnetometer. It has high reliability, strong versatility, high algorithm operation efficiency, simple operation, high accuracy and good practicality.

Description

Technical Field

[0001] The present invention relates to the technical field of magnetometer initial alignment, and in particular to a MEMS IMU initial alignment method based on nonlinear optimization of magnetic heading. Background Art

[0002] The magnetometer is affected by itself and the external environment, and its data output is unstable. Directly using the magnetometer output to calculate the magnetic heading for the initial alignment of a low-precision integrated inertial navigation system (hereinafter referred to as low-precision MEMS IMU) will result in large errors.

[0003] Currently, the initial alignment methods for low-precision MEMS IMUs are mainly divided into magnetometer-based initial alignment methods and GNSS-based initial alignment methods. For example, the published patent CN108535755B provides a MEMS-based GNSS / IMU vehicle-mounted real-time integrated navigation method, which dynamically aligns the IMU's initial attitude, position, and velocity information based on GNSS output information. However, this method requires high positioning accuracy of GNSS signals. The published patent CN112097763B provides an underwater vehicle integrated navigation method based on a MEMS IMU / magnetometer / DVL combination. This method directly uses the magnetic vector after coordinate rotation to calculate the magnetic heading for initial alignment. However, due to the error in the magnetometer output, the error in initial alignment using this magnetic heading is relatively large.

[0004] Therefore, in order to achieve accurate initial alignment based on low-precision MEMS IMU, it is necessary to optimize the initial magnetic heading calculated by the magnetometer to achieve accurate MEMS IMU initial alignment. Summary of the Invention

[0005] The purpose of the present invention is to provide a MEMS IMU initial alignment method based on nonlinear optimization of magnetic heading, which solves the problem of insufficient alignment accuracy of the existing MEMS IMU initial alignment method based on a magnetometer.

[0006] To this end, the technical solution of the present invention is as follows:

[0007] A MEMS IMU initial alignment method based on nonlinear optimization of magnetic heading, the steps are as follows:

[0008] S1. Determine the true magnetic vector and magnetic declination of the current position using the world geomagnetic model;

[0009] S2. Calculate the initial magnetic heading, pitch angle, and roll angle using the MEMS IMU.

[0010] S3. Construct a nonlinear optimized magnetic heading model, whose expression is:

[0011]

[0012] Where m w is the three-dimensional magnetic vector of the current position determined by step S1; m b is the three-dimensional magnetic vector output by the magnetometer in the MEMS IMU; is the rotation matrix from the carrier coordinate system to the world geomagnetic coordinate system, and its expression is: ψ, φ, and θ are the MEMS IMU initial magnetic heading, initial aligned roll, and initial aligned pitch angles, respectively;

[0013] S4. Solve the nonlinear optimization magnetic heading model. The steps are as follows:

[0014] S401, initializing nonlinear optimization parameters and λ;

[0015] S402. Calculate the Jacobian matrix Its expression is:

[0016]

[0017] Where γ is the rotation matrix Multiply a small amount on the left The corresponding Lie algebra, P is the rotation center of the IMU coordinate system;

[0018] S403. Calculate the Hessian matrix H, which is expressed as:

[0019] S404, calculate the increment of the conversion matrix Its expression is: Where λ is the damping factor, H is the Hessian matrix, g is the bias matrix; I is the third-order identity matrix;

[0020] S405: Update the conversion matrix Its expression is: Where, is the updated transformation matrix, is the transformation matrix substituted into the current calculation process;

[0021] S406, repeat the above steps S401 to S405 to iteratively update the conversion matrix until the nonlinear optimization magnetic heading model constructed in step S3 reaches a convergence state; at this time, the increment of the current conversion matrix That is the optimal rotation matrix increment Based on the optimal rotation matrix increment Calculate the optimal rotation matrix

[0022] S407: Optimal rotation matrix increment based on step S406 Convert to get the optimal initial alignment heading angle ψ best .

[0023] Furthermore, in step S1, the method for determining the true magnetic vector and magnetic declination of the current position is:

[0024] S101, calculate the three-dimensional magnetic vector m of the current position through the world geomagnetic model w and magnetic declination Δψ, which is expressed as:

[0025] [m w ,Δψ]=WWM(Lat0,Lon0,Alt0,t0),

[0026] Where m w is the three-dimensional magnetic vector calculated by the World Magnetic Model, Δψ is the magnetic declination calculated by the World Magnetic Model, WWM(i) is the World Magnetic Model function, Lat0, Lon0, Alt0, t0 are the latitude, longitude, altitude, and time of the current MEMS IMU location, respectively;

[0027] S102, using the three-dimensional magnetic vector m calculated from the world geomagnetic model w As the real magnetic vector at the current MEMS IMU position.

[0028] Furthermore, in step S2, the calculation expressions of the initial magnetic heading, pitch angle and roll angle are:

[0029]

[0030] Where, ψ m is the initial magnetic heading, mx b ,my b They represent the x-axis magnetic component and y-axis magnetic component output by the magnetometer in the MEMS IMU in the carrier coordinate system, θ is the pitch angle of the initial alignment of the MEMS IMU, φ is the roll angle of the initial alignment of the MEMS IMU, and a x ,a y ,a z They are the x-axis acceleration component, y-axis acceleration component, and z-axis acceleration component output by the accelerometer in the MEMS IMU.

[0031] Furthermore, in step S406, the number of iterations is generally set to be ≥100 times.

[0032] Furthermore, in step S407, based on the optimal rotation matrix obtained in step S406 Convert to get the optimal initial alignment heading angle ψ bestThe conversion expression is:

[0033] ψ best =tan -1 (C 21 / C 11 ),

[0034] Where, ψ best is the optimal initial alignment heading angle, C 11 is the optimal rotation matrix The element in the first row and first column of C 21 is the optimal rotation matrix The element in the second row and first column of .

[0035] Compared with the prior art, the MEMS IMU initial alignment method based on nonlinear optimized magnetic heading of the present invention solves the problem of insufficient alignment accuracy of the existing MEMS IMU initial alignment method based on a magnetometer. The method first calculates the true magnetic vector and magnetic declination of the current position using the world geomagnetic model, then calculates the initial magnetic heading, pitch angle, and roll angle using the MEMS IMU, then constructs a nonlinear optimized magnetic heading model, and finally solves the nonlinear optimized magnetic heading model to complete the MEMS IMU initial alignment. The method has high reliability, strong versatility, high algorithm operation efficiency, simple operation, high accuracy, and good practicality. BRIEF DESCRIPTION OF THE DRAWINGS

[0036] Figure 1 Flowchart of the MEMS IMU initial alignment method based on nonlinear optimization of magnetic heading of the present invention;

[0037] Figure 2 Flowchart of step S4 in the MEMS IMU initial alignment method based on nonlinear optimization of magnetic heading according to the present invention;

[0038] Figure 3 Schematic diagram of error bars before and after the initial alignment heading angle is optimized by the MEMS IMU initial alignment method based on nonlinear optimization of magnetic heading according to the present invention. DETAILED DESCRIPTION

[0039] The present invention will be further described below with reference to the accompanying drawings and specific embodiments, but the following embodiments are by no means intended to limit the present invention in any way.

[0040] See also Figure 1 The specific implementation steps of the MEMS IMU initial alignment method based on nonlinear optimization of magnetic heading are described as follows.

[0041] S1. Determine the true magnetic vector and magnetic declination of the current position using the world geomagnetic model;

[0042] The specific operation steps of step S1 are:

[0043] S101, calculate the three-dimensional magnetic vector m of the current position through the world geomagnetic model w and magnetic declination Δψ, which is expressed as:

[0044] [m w ,Δψ]=WWM(Lat0,Lon0,Alt0,t0),

[0045] Where m w is the three-dimensional magnetic vector calculated by the World Magnetic Model, Δψ is the magnetic declination calculated by the World Magnetic Model, WWM(i) is the World Magnetic Model function, Lat0, Lon0, Alt0, t0 are the latitude, longitude, altitude, and time of the current MEMS IMU location, respectively;

[0046] S102, using the three-dimensional magnetic vector m calculated from the world geomagnetic model w As the real magnetic vector at the current MEMS IMU position.

[0047] S2. Calculate the initial magnetic heading, pitch angle, and roll angle using the MEMS IMU.

[0048] The specific operation steps of step S2 are:

[0049] Calculate the initial magnetic heading ψ using the MEMS IMU m , pitch angle θ and roll angle φ, whose expressions are:

[0050]

[0051] Where, ψ m is the initial magnetic heading, mx b ,my b They represent the x-axis magnetic component and y-axis magnetic component output by the magnetometer in the MEMS IMU in the carrier coordinate system (hereinafter referred to as b-frame), θ is the pitch angle of the initial alignment of the MEMS IMU, φ is the roll angle of the initial alignment of the MEMS IMU, and a x ,a y ,a z They are the x-axis acceleration component, y-axis acceleration component, and z-axis acceleration component output by the accelerometer in the MEMS IMU.

[0052] S3, constructing a nonlinear optimized magnetic heading model;

[0053] The specific operation steps of step S3 are:

[0054] Based on the three-dimensional magnetic vector m set in step S1 wAs the true magnetic vector at the current MEMS IMU position, the true magnetic vector can be expressed as:

[0055] m w =[mx w ,my w ,mz w ] T ,

[0056] Where mx w ,my w ,mz w They represent the true three-axis magnetic components at the current MEMSIMU position in the world geomagnetic coordinate system (hereinafter referred to as w-frame).

[0057] Furthermore, the three-dimensional magnetic vector measured by the MEMS IMU and the true three-dimensional magnetic vector approximately satisfy the following relationship:

[0058]

[0059] Where m b is the three-dimensional magnetic vector output by the magnetometer in the MEMS IMU; mx b ,my b ,mz b They represent the x-axis magnetic component, y-axis magnetic component, and z-axis magnetic component output by the magnetometer in the MEMS IMU under b-frame, respectively. is the rotation matrix from b-frame to w-frame, and its expansion is:

[0060]

[0061] Where ψ is the heading angle of the MEMS IMU initial alignment, φ is the roll angle of the initial alignment, and θ is the pitch angle of the initial alignment;

[0062] Furthermore, the nonlinear optimization objective function of the heading angle ψ estimation is constructed As a nonlinear optimization magnetic heading model, its expression is:

[0063]

[0064] Where, This is the nonlinear optimization objective function for magnetic heading estimation. By continuously adjusting the magnetic heading calculated by the magnetometer to minimize the objective function, the precise initial heading angle can be obtained, completing the precise initial alignment of the MEMS IMU.

[0065] S4. Solve the nonlinear optimization magnetic heading model.

[0066] The specific operation steps of step S4 are:

[0067] S401, initialize nonlinear optimization parameters, including and λ; where

[0068] is the rotation matrix, which is initialized as Specifically, the magnetic heading ψ obtained in step S2 is m , the initial alignment roll angle φ and the initial alignment pitch angle θ are substituted into the rotation matrix expression in step S3 to calculate;

[0069] λ is the damping factor, which is a fixed value set according to the navigation data. This damping factor is used to control the step size of the objective function descent, making the solution of the search direction more stable, so as to achieve a more stable convergence effect than the Gauss-Newton method.

[0070] S402. Calculate the Jacobian matrix

[0071] The nonlinear optimization objective function constructed in step S3 Perform a first-order Taylor expansion, and the expression is:

[0072]

[0073] Where, It is for about The derivative of , which is a Jacobian matrix; is the rotation matrix increment;

[0074] Since the rotation matrix is ​​a 3rd-order square matrix, it is not possible to directly perform derivative operations on it. Based on this, this application converts the rotation matrix into a Lie algebra through the conversion relationship between Lie groups and Lie algebras, and then performs nonlinear optimization iterations to calculate the optimal rotation matrix.

[0075] Using the perturbation model, the rotation matrix Multiply a small amount on the left, and then solve the rate of change of the Lie algebra relative to this small amount, which completes the rotation matrix The derivative of is:

[0076]

[0077] Where γ is the rotation matrix Multiply a small amount on the left The corresponding Lie algebra, P is the rotation center of the IMU coordinate system;

[0078] S403, calculate the Hessian matrix H:

[0079] The core goal of nonlinear optimization is to find Make minimum; and then Transformed into the following linear least squares relationship:

[0080]

[0081] Where, is the optimal rotation matrix increment;

[0082] Through the above least squares relationship Taking the derivative and setting it to zero, we can get the following constraint equation:

[0083]

[0084] In the above constraint equation, the Hessian matrix H is the coefficient on the left side of the equal sign, that is The bias matrix g is the coefficient on the right side of the above equation, that is

[0085] Then, the incremental equation of the Gauss-Newton method is obtained, which is expressed as follows:

[0086]

[0087] According to the Jacobian matrix obtained in step S402 Calculate H and g respectively;

[0088] S404, calculate the increment of the conversion matrix

[0089] In the Gauss-Newton algorithm, since the Hessian matrix H is approximated by the Jacobian matrix, it cannot be guaranteed to be reversible, which may lead to an incorrect increment in the calculation. Therefore, to solve this problem, the Levenberg-Marquadt method is used to introduce a parameter, namely the damping factor λ, to enhance the positive definiteness of the Hessian matrix H.

[0090] The expression of the incremental equation of the Levenberg-Marquadt method is:

[0091]

[0092] Where λ is the damping factor, H is the Hessian matrix, g is the bias matrix, and I is the third-order identity matrix, which is expressed as follows:

[0093] Substitute H and g calculated in step S404 into the above incremental equation to calculate the incremental transformation matrix

[0094] S405: Update the conversion matrix

[0095] The increment of the conversion matrix obtained in step S404 is Substitute into the transformation matrix update formula:

[0096]

[0097] Where, is the updated transformation matrix, is the transformation matrix substituted into the current calculation process; in the first calculation, Substitute the initialization matrix And in the next calculation, Substitute the updated transformation matrix

[0098] S406, repeat the above steps S401 to S405 to iteratively update the conversion matrix until the nonlinear optimization magnetic heading model constructed in step S3, that is, the nonlinear optimization objective function Reaching the convergence state; at this time, the increment of the current transformation matrix That is the optimal rotation matrix increment

[0099] Then, based on the optimal rotation matrix increment Calculate the optimal rotation matrix Its expression is: Where, Substitute the transformation matrix substituted in the current iteration process;

[0100] In step S406, the number of iterations is generally set to ≥100 times;

[0101] S407: Based on the optimal rotation matrix obtained in step S406 Convert to get the optimal initial alignment heading angle ψ best , its conversion expression is:

[0102] ψ best =tan -1 (C 21 / C 11 ),

[0103] Where, ψ best is the optimal initial alignment heading angle, C 11 is the optimal rotation matrix The element in the first row and first column of C 21 is the optimal rotation matrix The element in the second row and first column of .

[0104] After the above steps S401 to S407, the optimization of the initial alignment heading angle based on the MEMS IMU is completed.

[0105] Furthermore, in order to verify the correctness of the present invention, actual measurement experiments were carried out; wherein, the performance parameters of the magnetometer in the experiment are listed in Table 1, and the performance parameters of the gyroscope and accelerometer in the experiment are listed in Table 2.

[0106] Table 1:

[0107] index Range noise Nonlinear Resolution parameter ±8Gs 1mGs 0.2％ 0.25mGs

[0108] Table 2:

[0109]

[0110] Data was collected based on the magnetometer with the performance parameters shown in Table 1 and the gyroscope and accelerometer with the performance parameters shown in Table 2 to repeat three sets of alignment experiments; initial alignment was performed using the method of the present invention, and initial alignment without using the method of the present invention was used as a comparative example.

[0111] like Figure 3 The figure shows a schematic diagram of the heading error bars obtained from three sets of repeated alignment experiments. From this figure, it can be clearly seen that before and after optimization, the maximum value (the upper black horizontal line of the error bar), the minimum value (the lower black horizontal line of the error bar), and the mean value (the middle blue horizontal line of the error bar) of the initial alignment heading angle error are obtained.

[0112] In the three groups of experiments, the heading angle error test results of the front and rear heading alignment accuracy optimized by the method of this application and the calculation results of the improvement percentage of the alignment accuracy are shown in Table 2 below.

[0113] Table 2:

[0114]

[0115] from Figure 3 Comparing the heading alignment accuracy before and after nonlinear optimization in Table 2 shows that the method of the present invention can achieve higher-precision MEMS IMU initial alignment. Compared with the unoptimized heading alignment method, the heading alignment accuracy of the present invention is improved by approximately 40%. This verifies the effectiveness and correctness of the method provided by the present invention.

[0116] Portions of the present invention not disclosed in detail are known in the art. Although the above description of illustrative embodiments of the present invention is intended to facilitate understanding of the present invention by those skilled in the art, it should be understood that the present invention is not limited to the scope of the specific embodiments. As long as various modifications are obvious to those skilled in the art within the spirit and scope of the present invention as defined and determined by the appended claims, all inventions and creations utilizing the concepts of the present invention are protected.

Claims

1. A MEMSIMU initial alignment method based on nonlinear optimization of magnetic heading, characterized in that: Here are the steps: S1. Determine the true magnetic vector and magnetic declination of the current position using the world geomagnetic model; S2. Calculate the initial magnetic heading, pitch angle, and roll angle using MEMSIMU. S3. Construct a nonlinear optimized magnetic heading model, whose expression is: Where m w is the three-dimensional magnetic vector of the current position determined by step S1; m b is the three-dimensional magnetic vector output by the magnetometer in MEMSIMU; is the rotation matrix from the carrier coordinate system to the world geomagnetic coordinate system, and its expression is: ψ, φ, and θ are the MEMSIMU initial magnetic heading, initial aligned roll, and initial aligned pitch angles, respectively; S4. Solve the nonlinear optimization magnetic heading model. The steps are as follows: S401, initializing nonlinear optimization parameters and λ; S402. Calculate the Jacobian matrix Its expression is: Where γ is the rotation matrix Multiply a small amount on the left The corresponding Lie algebra, P is the rotation center of the IMU coordinate system; S403. Calculate the Hessian matrix H, which is expressed as: S404, calculate the increment of the conversion matrix Its expression is: Where λ is the damping factor, H is the Hessian matrix, g is the bias matrix; I is the third-order identity matrix; S405: Update the conversion matrix Its expression is: Where, is the updated transformation matrix, is the transformation matrix substituted into the current calculation process; S406, repeat the above steps S401 to S405 to iteratively update the conversion matrix until the nonlinear optimization magnetic heading model constructed in step S3 reaches a convergence state; at this time, the increment of the current conversion matrix That is the optimal rotation matrix increment Based on the optimal rotation matrix increment Calculate the optimal rotation matrix S407: Based on the optimal rotation matrix obtained in step S406 Convert to get the optimal initial alignment heading angle ψ best .

2. The MEMSIMU initial alignment method based on nonlinear optimization of magnetic heading according to claim 1, characterized in that: In step S1, the method for determining the true magnetic vector and magnetic declination of the current position is: S101, calculate the three-dimensional magnetic vector m of the current position through the world geomagnetic model w and magnetic declination Δψ, which is expressed as: [m w ,Δψ]=WWM(Lat0,Lon0,Alt0,t0), Where m w is the three-dimensional magnetic vector calculated by the world magnetic model, Δψ is the magnetic declination calculated by the world magnetic model, WWM(i) is the world magnetic model function, Lat0, Lon0, Alt0, t0 are the latitude, longitude, altitude and time of the current MEMSIMU location respectively; S102, using the three-dimensional magnetic vector m calculated from the world geomagnetic model w As the real magnetic vector at the current MEMSIMU position.

3. The MEMSIMU initial alignment method based on nonlinear optimization of magnetic heading according to claim 1, characterized in that: In step S2, the calculation expressions of the initial magnetic heading, pitch angle and roll angle are: Where, ψ m is the initial magnetic heading, mx b ,my b They represent the x-axis magnetic component and y-axis magnetic component output by the magnetometer in the MEMSIMU in the carrier coordinate system, θ is the pitch angle of the initial alignment of the MEMSIMU, φ is the roll angle of the initial alignment of the MEMSIMU, and a x ,a y ,a z They are the x-axis acceleration component, y-axis acceleration component and z-axis acceleration component output by the accelerometer in MEMSIMU respectively.

4. The MEMSIMU initial alignment method based on nonlinear optimization of magnetic heading according to claim 1, characterized in that: In step S406, the number of iterations is generally set to ≥100 times.

5. The MEMSIMU initial alignment method based on nonlinear optimization of magnetic heading according to claim 1, characterized in that: In step S407, based on the optimal rotation matrix increment obtained in step S406 Convert to get the optimal initial alignment heading angle ψ best The conversion expression is: ψ best =tan -1 (C 21 / C 11 ), Where, ψ best is the optimal initial alignment heading angle, C 11 is the optimal rotation matrix The element in the first row and first column of C 21 is the optimal rotation matrix The element in the second row and first column of .

Images