A MEMS gyroscope north-seeking method and device based on a hybrid algorithm

Abstract

The application discloses a MEMS gyroscope north-seeking method and device based on a hybrid algorithm, which comprises the following steps: firstly, collecting three-axis acceleration and gyroscope data of a north seeker at any two positions; calculating the pitch angle and the roll angle of the two positions according to the acceleration data; then, establishing a function relation model containing the azimuth angle and the gyroscope zero offset based on the coordinate transformation and the earth rotation angular velocity, and constructing a nonlinear optimization equation with the two azimuth angles as optimization variables by using the characteristic that the zero offset is short-time invariable; finally, performing global search by using an ant colony optimization algorithm to obtain a rough solution, taking the solution as an initial value, and performing local accurate solution by using the steepest descent method to obtain a high-precision azimuth angle result. The application can realize north-seeking at any two positions without relying on a precise rotating device, and has the advantages of simple structure, convenient operation, high north-seeking precision and high calculation efficiency.

Description

Technical Field

[0001] This invention belongs to the technical field of gyroscope north finding, specifically relating to a MEMS gyroscope north finding method and device based on a hybrid algorithm. Background Technology

[0002] Inertial north-finding systems are highly reliable and stable, and are unaffected by external environmental factors such as weather, topography, day / night cycle, and magnetic fields, except for limitations imposed by high latitudes. They are widely used in launching devices, well logging, geological exploration, and other fields.

[0003] Currently, most north-finding instruments employ two-position, four-position, or multi-position north-finding methods. These methods suffer from limitations such as reliance on high-precision indexing devices and limited operation to near-horizontal positions; or they require the indexing device to rotate precisely around the Z-axis of the north-finding instrument and output the relative angle information between the two positions, resulting in a complex system structure. Single-position north-finding methods, on the other hand, suffer from low accuracy due to the zero-bias drift of the MEMS gyroscope itself. Therefore, researching a method that does not rely on indexing devices and can achieve north-finding in any two positions is of great significance.

[0004] The north-finding solution at any two positions can be transformed into solving a nonlinear optimization problem. The conventional steepest descent method depends to some extent on the initial value, while the ant colony algorithm is computationally time-consuming and not conducive to engineering implementation. Summary of the Invention

[0005] In view of this, the present invention proposes a MEMS gyroscope north-finding method and device based on a hybrid algorithm, which can complete the north-finding by collecting the output data of MEMS gyroscopes at any two positions. The method has low computational load and short time consumption while ensuring the accuracy of north-finding, and has strong engineering practical value.

[0006] The technical solution for implementing the present invention is as follows: A MEMS gyroscope north-finding method based on a hybrid algorithm includes the following steps: Step 1: Place the north finder in two different positions and collect data from the three-axis accelerometer and the three-axis MEMS gyroscope when the north finder is in position 1 and position 2. Step 2: Based on the triaxial acceleration data collected at the two locations, calculate the pitch and roll angles of the north finder at the two locations respectively; Step 3: Based on the pitch angle, roll angle and Earth rotation rate, establish a correlation model between the output of the three-axis MEMS gyroscope at the two positions and the azimuth angle to be determined. The correlation model includes the zero bias of the three-axis MEMS gyroscope as an unknown parameter. Step 4: Based on the characteristic that the zero bias of the three-axis MEMS gyroscope remains unchanged during the measurement at the two positions, construct an objective function with the two azimuth angles as optimization variables using the correlation model; Step 5: Use the ant colony optimization algorithm to perform global optimization on the objective function to obtain the initial estimate of the azimuth angle; Step Six: Starting from the initial estimated value, use the steepest descent method to perform a local exact solution to the objective function to obtain the final result of the azimuth angle.

[0007] Furthermore, in step two, the pitch angle is obtained by calculating the arcsine function of the acceleration measurement value on the forward axis component of the carrier; the roll angle is obtained by calculating the arctangent function of the ratio of the acceleration measurement value on the lateral axis and vertical axis components of the carrier.

[0008] Furthermore, in step three, the correlation model specifically refers to: using a coordinate rotation matrix, expressing the three-axis angular rate measured by the north finder in the carrier coordinate system as a function of the projection of the Earth's rotation angular rate in the local geographic coordinate system, the pitch angle, the roll angle, the azimuth angle, and the three-axis zero bias of the three-axis MEMS gyroscope.

[0009] Further, in step four, the construction of the objective function is specifically as follows: combining the two correlation models at positions 1 and 2, under the constraint that the three axes of the three-axis MEMS gyroscope have the same zero bias, the objective is to minimize the sum of squares of the differences between the theoretical output value and the measured value of the gyroscope at the two positions, thereby forming a nonlinear optimization equation about the two azimuth angles.

[0010] Furthermore, in step five, the ant colony optimization algorithm is implemented using particle swarm optimization, where the fitness function is negatively correlated with the objective function, and a linearly decreasing inertia weight strategy is used to update the position and velocity of the particles during the iteration process.

[0011] Furthermore, in step six, the steepest descent method uses the negative gradient direction of the objective function as the search direction and iteratively calculates to find the optimal step size that makes the objective function value decrease until the preset convergence condition is met.

[0012] A MEMS gyroscope north-finding device based on a hybrid algorithm includes: The data acquisition module is used to acquire the output data of the three-axis accelerometer and the three-axis MEMS gyroscope when the north finder is at position 1 and position 2. Processing unit, used to execute the steps of the above method; The data acquisition module and the processing unit are integrated into the hardware circuitry inside the north-finding instrument.

[0013] Beneficial effects: 1. The north-finding method proposed in this invention does not rely on a high-precision indexing device or a complex system for accurately measuring the relative angle between two positions. It only requires manually placing the north-finding instrument at two arbitrary positions. This significantly reduces hardware complexity, manufacturing costs, and stringent requirements on the operating environment (such as levelness), thereby improving the system's reliability and practicality.

[0014] 2. This invention constructs a nonlinear optimization model that includes gyroscope zero bias and innovatively adopts a hybrid solution strategy of "ant colony optimization algorithm (global search) combined with steepest descent method (local refinement)," which effectively overcomes the shortcomings of traditional single-position method being greatly affected by zero bias and single optimization algorithm (such as steepest descent method) being sensitive to initial value or (such as ant colony algorithm) having low computational efficiency.

[0015] 3. The hybrid algorithm of this invention fully leverages the advantages of both algorithms: the ant colony algorithm can effectively find high-quality coarse solutions within a large solution space, providing good initial values ​​for subsequent refinement; the steepest descent method, based on this, quickly converges to the exact solution. This strategy, while ensuring north-finding accuracy, significantly reduces the long computation time of the pure ant colony algorithm, making the algorithm more suitable for engineering applications with real-time requirements.

[0016] 4. The method of this invention only requires collecting sensor data from two arbitrary placement positions of the north finder to complete the calculation, making the operation process simple and quick. It eliminates the dependence on precision mechanical rotation processes and strict rotation axes, making the north finding process more flexible and convenient, and broadening its application potential in complex environments such as field, mobile platforms, or space-constrained environments. Attached Figure Description

[0017] Figure 1 This is a flowchart of the method of the present invention.

[0018] Figure 2 This is a schematic diagram showing the position of the north-finding instrument relative to the carrier coordinate system. Detailed Implementation

[0019] As attached Figure 1 As shown, this invention proposes a MEMS gyroscope north-finding method based on a hybrid algorithm. It collects the output data of the three-axis gyroscope in two different positions of the north finder. It does not require the use of a rotation device or the acquisition of the attitude angle change between the two positions. The north finder has a simple structure, a simple north-finding method, and high north-finding accuracy.

[0020] The north-finding instrument used in this invention consists of a three-axis MEMS accelerometer (Acc), a three-axis MEMS gyroscope (Wx, Wy, Wz), and a corresponding data acquisition and processing system. The three-axis MEMS accelerometer is used to measure acceleration data in three axes, and the three-axis MEMS gyroscope is used to measure angular rate data in three axes.

[0021] In this invention, the geographic coordinate system refers to the northeast-northeast coordinate system. The X-axis of the geographic coordinate system points due north, the Y-axis is perpendicular to the X-axis and points due east, and the Z-axis points vertically downwards. The carrier coordinate system refers to a coordinate system fixed relative to the carrier platform. The north-finding instrument is installed along the carrier coordinate system. The X-axis of the carrier coordinate system points directly forward along the longitudinal axis of the carrier, the Y-axis points to the right along the transverse axis of the carrier, and the Z-axis points downwards. The X, Y, and Z axes form a right-handed coordinate system, as shown in the attached diagram. Figure 2 As shown. The azimuth measurement range is 0° to 360°, with 0° when pointing due north and clockwise rotation being positive; the pitch measurement range is -90° to 90°, with 0° when horizontal, upward tilt being positive and downward tilt being negative; the roll measurement range is -180° to 180°, with 0° when horizontal, rightward tilt being positive and leftward tilt being negative. The north-finding instrument's length direction is the forward direction, corresponding to the X-axis of the three-axis gyroscope and the A-axis of the three-axis accelerometer; the north-finding instrument's rightward direction corresponds to the Y-axis of the three-axis gyroscope and the B-axis of the three-axis accelerometer; the north-finding instrument's vertical downward direction corresponds to the Z-axis of the three-axis gyroscope and the C-axis of the three-axis accelerometer. These three directions form a right-handed coordinate system.

[0022] Based on the Euler rotation relation, the matrix representation of the transformation relationship between the carrier coordinate system (b-frame) and the geographic coordinate system (n-frame) can be obtained: (1) in, For pitch angle, For roll angle, This is the azimuth angle.

[0023] Pitch angle Roll angle The calculation method is as follows: the projection of the Earth's gravitational acceleration onto the horizontal direction in the geographic coordinate system is 0, and the value in the vertically downward direction is g. Then: (2) In the formula, The values ​​of the three-axis accelerations measured in the coordinate system of the north-finding instrument carrier are used to determine the values ​​of the accelerations. Let (2) be the coordinate rotation matrix from the geographic coordinate system to the carrier coordinate system. Expanding equation (2) yields: (3) The pitch angle can be calculated by solving equation (3). Roll angle : (4) (5) Similarly, the theoretical values ​​of the three-axis angular rates measured by the north-finding instrument in the carrier coordinate system It can be obtained through the following formula (6): (6) in, , These are the projections of the Earth's rotation angular rate in the due north direction and the vertically downward direction in the geodetic coordinate system, respectively. This is the Earth's rotational angular rate. The latitude is the local latitude.

[0024] Both the random drift of the MEMS gyroscope and the error output of the MEMS accelerometer will affect the north-finding accuracy. According to calculations, the accuracy of the MEMS accelerometer only needs to reach 1mg, and the resulting north-finding error can be ignored. Therefore, this patent only considers the influence of the random drift of the MEMS gyroscope.

[0025] Expanding equation (6) yields the three-axis angular rates measured by the three-axis MEMS gyroscope at position 1: (7) The triaxial angular velocities measured at position 2: (8) in, , , To determine the azimuth, elevation, and roll angles of the north-finding instrument at position 1, , , To determine the azimuth, elevation, and roll angles of the north-finding instrument at position 2; , , To find the zero bias of the X, Y, and Z axes of the three-axis gyroscope in the Beidou instrument, , , This is the measured value of the three-axis gyroscope at position 1 of the north-finding instrument. , , This is the theoretical output of the three-axis gyroscope at position 1 for the north-finding instrument. , , The measured value of the three-axis gyroscope at position 2 is the north-finding instrument. , , This is the theoretical output of the three-axis gyroscope at position 2 for the north-finding instrument.

[0026] The unknowns in the above formula include the azimuth angles of two locations. , and three gyroscopes with zero bias , , Based on the characteristic that the gyroscope remains unchanged for short periods of zero bias, solving for the azimuth angle of the north finder at the two positions can be transformed into the following nonlinear optimization problem: (9) To address the aforementioned nonlinear optimization problem, an ant colony optimization algorithm is first used to obtain the azimuth angles of the north-finding instrument at the two locations. , A rough explanation.

[0027] The steps for implementing the ant colony optimization algorithm are as follows: Step 1: Initialize the particle swarm, i.e., randomly assign the initial positions and initial velocities of m particles; The initialization process of the particle swarm optimization algorithm is as follows: 1) Set the population size m=32, the particle search space dimension to 2, and the maximum number of iterations. Current iteration count ; 2) For any , ,exist The inner elements are generated according to a random distribution. ; 3) For any , ,exist The inner elements are generated according to a random distribution. ; 4) For any ,set up ; 5) Find the initial population .

[0028] Step 2: Given a fitness function, calculate the fitness value (Fitness) for each particle; (10) Step 3: Determine the inertia weight value for each iteration; (11) in, This represents the maximum inertial weight value. This represents the minimum inertia weight value. Indicates the maximum number of iterations. This indicates the current iteration number.

[0029] Step 4: For each particle, compare its fitness value with the fitness value of its best position pi. If it is better, update pi. This is the optimal solution for the individual searched so far; Step 5: For each particle, compare its fitness value with the fitness value of the best position in the population, pg. If it is better, update pg. This is the optimal solution found so far for the entire population; Step 6: Update the position and speed of the ant colony; (12) (13) in For particles The velocity represents the particle's speed. The distance between the current location and its next target location; It is a particle The current location; It is a coefficient that maintains the original velocity, called inertial weight, and its magnitude determines how much of the particle's current velocity is inherited. It is the weighting coefficient for a particle to track its own historical best value. It represents the particle's own understanding and is called the cognitive constant. It is the weighting coefficient for the optimal value of particle tracking swarm. It represents the particle's knowledge of the entire swarm and is called the social knowledge constant. , It is a random number uniformly distributed within the interval [0,1]. Meanwhile, and It is constantly updated during the evolutionary process, and the final output is This is the optimal solution obtained by the algorithm.

[0030] Step 7: Determine if the stopping condition (a sufficiently good position or the maximum number of iterations) is met. If the stopping condition is met, output the optimal value; otherwise, go to Step 2 and continue running until the condition is met.

[0031] By using the ant colony optimization algorithm, rough solutions for the azimuth angles of the north finder at the two locations were calculated.

[0032] After obtaining the rough solutions of the azimuth angles of the north finder at the two positions, the precise solutions of the azimuth angles of the north finder at the two positions are calculated by the steepest descent method.

[0033] The steepest descent method is implemented in the following steps: First, the results obtained from the ant colony algorithm As initial values, calculate the search direction and step size: Search direction: This is the direction of the negative gradient.

[0034] Search step size along the negative gradient direction:

[0035] Start the search along the negative gradient direction:

[0036] Recalculate the search direction:

[0037] Search step size:

[0038] Start the loop iterative process until the condition is met. ,in Given a threshold.

[0039] This invention conducted multiple sets of repeatable tests at any two locations. At location 1, the actual azimuth, pitch, and roll angles were 30°, -0.8°, and 0.5°, respectively; at location 2, the actual azimuth, pitch, and roll angles were 150°, 30.8°, and -20.5°, respectively. Six sets of data were collected from the three-axis accelerometer and three-axis gyroscope at each of the two locations. The azimuth errors at location 1 obtained from the six calculations were 0.126°, 0.201°, 0.050°, -0.231°, 0.135°, and -0.059°, respectively; and the azimuth errors at location 2 obtained from the six calculations were 0.083°, 0.108°, -0.164°, -0.116°, 0.255°, and 0.121°, respectively. The test results show that this invention can achieve a north-finding accuracy better than 0.3°.

[0040] In summary, the above are merely preferred embodiments of the present invention and are not intended to limit the scope of protection of the present invention. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. A MEMS gyroscope north-finding method based on a hybrid algorithm, characterized in that, Includes the following steps: Step 1: Place the north finder in two different positions and collect data from the three-axis accelerometer and the three-axis MEMS gyroscope when the north finder is in position 1 and position 2. Step 2: Based on the triaxial acceleration data collected at the two locations, calculate the pitch and roll angles of the north finder at the two locations respectively; Step 3: Based on the pitch angle, roll angle and Earth rotation rate, establish a correlation model between the output of the three-axis MEMS gyroscope at the two positions and the azimuth angle to be determined. The correlation model includes the zero bias of the three-axis MEMS gyroscope as an unknown parameter. Step 4: Based on the characteristic that the zero bias of the three-axis MEMS gyroscope remains unchanged during the measurement at the two positions, construct an objective function with the two azimuth angles as optimization variables using the correlation model; Step 5: Use the ant colony optimization algorithm to perform global optimization on the objective function to obtain the initial estimate of the azimuth angle; Step Six: Starting from the initial estimated value, use the steepest descent method to perform a local exact solution to the objective function to obtain the final result of the azimuth angle.

2. The MEMS gyroscope north-finding method based on a hybrid algorithm according to claim 1, characterized in that, In step two, the pitch angle is obtained by calculating the arcsine function of the acceleration measurement value on the forward axis component of the carrier; the roll angle is obtained by calculating the arctangent function of the ratio of the acceleration measurement value on the lateral axis and vertical axis components of the carrier.

3. The MEMS gyroscope north-finding method based on a hybrid algorithm according to claim 1, characterized in that, In step three, the correlation model is specifically as follows: through the coordinate rotation matrix, the three-axis angular rate measured by the north finder in the carrier coordinate system is expressed as a function of the projection of the Earth's rotation angular rate in the local geographic coordinate system, the pitch angle, the roll angle, the azimuth angle, and the three-axis zero bias of the three-axis MEMS gyroscope.

4. The MEMS gyroscope north-finding method based on a hybrid algorithm according to claim 2, characterized in that, In step four, the objective function is constructed as follows: by combining the two correlation models at positions 1 and 2, and under the constraint that the three axes of the three-axis MEMS gyroscope have the same zero bias, the objective is to minimize the sum of squares of the differences between the theoretical output value and the measured value of the gyroscope at the two positions, thereby forming a nonlinear optimization equation for the two azimuth angles.

5. The MEMS gyroscope north-finding method based on a hybrid algorithm according to claim 1, characterized in that, In step five, the ant colony optimization algorithm is implemented using particle swarm optimization. Its fitness function is negatively correlated with the value of the objective function, and a linearly decreasing inertia weight strategy is used to update the position and velocity of the particles during the iteration process.

6. The MEMS gyroscope north-finding method based on a hybrid algorithm according to any one of claims 1-5, characterized in that, In step six, the steepest descent method uses the negative gradient direction of the objective function as the search direction and iteratively calculates to find the optimal step size that makes the objective function value decrease until the preset convergence condition is met.

7. A MEMS gyroscope north-finding device based on a hybrid algorithm, characterized in that, include: The data acquisition module is used to acquire the output data of the three-axis accelerometer and the three-axis MEMS gyroscope when the north finder is at position 1 and position 2. Processing unit, configured to perform the steps of the method as described in any one of claims 1 to 7; The data acquisition module and the processing unit are integrated into the hardware circuitry inside the north-finding instrument.

Images