Fast initial heading alignment method, system and terminal for strapdown inertial navigation system

Abstract

The invention belongs to the technical field of initial alignment of an inertial navigation system, and discloses a method, system and terminal for fast initial heading alignment of a strapdown inertial navigation system, initializes random particle variables, sets a fitness function, substitutes particle positions into the fitness function to obtain fitness values, and compares the fitness values ​​of all particles to judge the quality of particle positions; in each iteration, the positions of all particles track the optimal position according to certain evolutionary rules until the iteration reaches a preset number of times; a PSO method is applied to the initial heading alignment of a strapdown inertial navigation system, and the initial attitude is used as the optimization target of a particle swarm, while the accelerometer zero bias is included in the optimization, and the velocity error root mean square is used as the fitness function. The present invention uses a particle swarm to directly optimize the initial attitude of a strapdown inertial navigation system, does not need to perform mathematical modeling on the system and observation noise, avoids the decrease in alignment accuracy caused by inaccurate modeling, and improves the rapidity and accuracy of alignment.

Description

Technical Field

[0001] The invention belongs to the technical field of initial alignment of an inertial navigation system, and in particular relates to a method, a system and a terminal for rapid initial heading alignment of a strapdown inertial navigation system. Background Art

[0002] At present, the strapdown inertial navigation system must rely on the initial alignment process to enter normal operation. The reason is that the inertial navigation system is an integral calculation system in principle, and the output of its sensor needs to be integrated and solved. Therefore, before entering the integral solution, the initial value of the integral needs to be determined, that is, the initial alignment needs to be performed. Initial alignment is the premise and guarantee for the normal operation of the inertial navigation system. Any initial alignment error will enter the integral calculation and cause subsequent system errors. Therefore, initial alignment technology has become an important research direction in the field of inertial navigation technology. The initial alignment of the strapdown inertial navigation system requires determining the initial position, attitude and heading information of the vehicle. Among them, the initial heading information is the most difficult to obtain, so heading alignment is the key to the entire alignment process.

[0003] The existing initial alignment technology is mainly based on the optimal estimation Kalman filter (KF) initial alignment. The so-called filtering refers to extracting the required effective signal from a complex mixed signal, while KF estimates the required signal by observing the relevant external observables. The prerequisite for its use is that the state equation and measurement equation of the system are known, where the former describes the functional relationship between the system excitation and the system response; the latter describes the functional relationship between the observable and the state to be estimated. In each filtering cycle, there are two steps: state prediction and measurement update. State prediction uses historical information and the state estimation of the previous step to make a priori estimate of the current state, while measurement update uses the functional relationship between the external quantity measurement and the estimated state quantity to correct the state prior estimate and obtain the state estimate. Prediction represents past experience information, while measurement represents immediate reference information. The weights of these two steps in state estimation are adjusted by the filter gain.

[0004] The above process has the following problems or shortcomings in practical application:

[0005] 1. When performing KF filtering, it is necessary to obtain relatively accurate statistical characteristics of system noise and observation noise in advance and model them. However, any preset noise model has certain inaccuracies. This inaccuracy is mainly caused by the following reasons: (1) approximate terms in the actual system modeling process; (2) equipment aging, device function degradation, etc. will cause changes in noise characteristics; (3) KF requires observation of external auxiliary information, and factors such as different working conditions of measurement equipment and changes in measurement environment will affect the observed noise characteristics; (4) unpredictable external interference during system operation will also cause system state changes. The above modeling inaccuracies will cause the filtering results to be unstable or even divergent, resulting in poor alignment accuracy.

[0006] 2. The essence of KF initial alignment is a recursive operation to obtain the most accurate estimate. Therefore, in general, to ensure a high initial alignment accuracy, a certain amount of alignment time must be paid. Usually, the initial alignment based on KF takes several minutes to align. During the alignment stage, the vehicle cannot perform the task normally, which restricts the vehicle's dispatch speed. Therefore, how to obtain a higher-precision alignment result in a shorter time (fast initial alignment) needs further research.

[0007] Through the above analysis, the problems and defects of the prior art are as follows:

[0008] (1) When the strapdown inertial navigation system uses the KF method for initial alignment, it is necessary to obtain accurate statistical characteristics of the filtering noise and model it in advance. However, any preset noise model has certain errors, and such inaccurate modeling will lead to unstable or even divergent filtering results, which will eventually lead to poor alignment accuracy.

[0009] (2) The initial alignment process based on the KF method is very sensitive to external measurement noise and is easily interfered by complex noise, resulting in unreliable alignment results;

[0010] (3) The initial alignment based on KF usually takes a long time to calculate, and the vehicle cannot perform its mission normally during the alignment stage, which restricts the vehicle's dispatching speed;

[0011] (4) The existing initial alignment scheme still has much room for improvement in terms of speed and accuracy. Slow alignment speed and poor alignment accuracy will affect the working efficiency and performance of the inertial navigation system, increase the time and economic cost of the inertial navigation system in application, and affect its benefits. Summary of the invention

[0012] In view of the problems existing in the prior art, the present invention provides a method, system and terminal for rapid initial heading alignment of a strapdown inertial navigation system.

[0013] The present invention is achieved in that a strapdown inertial navigation system fast initial heading alignment method comprises:

[0014] Initialize random particle variables, set the fitness function, substitute the particle position into the fitness function to get the fitness value, and compare the fitness values ​​of all particles to judge the quality of the particle position; in each iteration, the positions of all particles track the optimal position according to certain evolutionary rules until the iteration reaches the preset number of times; apply the PSO method to the initial heading alignment of the strapdown inertial navigation system, take the initial attitude as the optimization target of the particle swarm, and include the accelerometer zero bias in the optimization, and take the root mean square of the velocity error as the fitness function.

[0015] Further, the strapdown inertial navigation system fast initial heading alignment method comprises the following steps:

[0016] Step 1: Under a static base state, collect raw data output by an inertial measurement element in a strapdown inertial navigation system;

[0017] Step 2: using the global positioning system GPS to determine the longitude λ and latitude L of the initial position of the strapdown inertial navigation system;

[0018] Step 3: Initialize N particles in the feasible solution space to form an initial population, and randomly generate the position and velocity of the particles, where N ≥ 20;

[0019] Step 4: Taking the mean square error of the speed error output by the strapdown inertial navigation as the objective function, the position of each particle is substituted into the inertial navigation system solution equation to solve the speed, and the speed error root mean square solved by the inertial navigation is used as the objective function to calculate the particle fitness value;

[0020] Step 5: Compare the fitness values ​​of the particles, select the current individual optimal value and group optimal value of the particle with the smaller fitness value.

[0021] Step 6: Update the position and velocity of the particles and return to step 4 for loop iteration until the number of iterations reaches G. At this time, the optimal value of the particle group is the alignment result, where G ≥ 100.

[0022] Furthermore, the inertial measurement element in step 1 includes a gyroscope and an accelerometer; the sampling time of the raw data of the inertial measurement element is set to 100 s, and the sampling frequency is set to 1 Hz.

[0023] In step 3, the current position of the i-th (i=1, 2, ..., N) particle is x i =[x i1 ,x i2 ], where x i1 represents the inertial navigation direction, x i2represents the accelerometer zero bias, and the particle flying speed is v i =[v i1 ,v i2 ].

[0024] Furthermore, in step 4, the x in each particle position is i1 As the heading value, x i2 Substitute the accelerometer zero bias value into the inertial navigation solution equation to obtain the velocity v′ calculated by the inertial navigation solution. n (n=1,2,...,100); v′ n =[v′ nE ,v′ nN ], where v′ nE , v′ nN They represent the eastward speed and northward speed calculated by the inertial navigation respectively.

[0025] The objective function is designed as:

[0026]

[0027] The calculation result is the fitness value of each particle.

[0028] Furthermore, in step 5, after iteration, the optimal solution currently found by each particle is set to p i =[p i1 ,p i2 ], the global optimal solution currently found by the population is p g =[p g1 ,p g2 ].

[0029] Furthermore, in step 6, the particle position and velocity update equations are:

[0030] x i+1 =x i +v i ;

[0031] v i+1 =ωv i +c1R1(p i -x i )+c2R2(p g -x i );

[0032] Where c1 and c2 are acceleration coefficients or learning factors, which are used to enable particles to have the ability to self-learn and learn from excellent individuals in the population, and their values ​​are 2; R1 and R2 are random numbers uniformly distributed between [0,1] and are independent of each other; ω is the inertia weight, which is used to determine the degree of influence of the previous velocity of the particle on the current velocity and balance the global search and local search capabilities of the algorithm.

[0033] The particle velocity is limited to [V max ,V min ], set V max =[0.1rad,1ug], V min =-[0.1rad,1ug], where rad is the heading angle unit, representing "radians"; ug is the accelerometer zero bias unit, 1ug ​​= 1×10 -6 g, g represents the acceleration due to gravity, which is 9.78m / s 2 After executing the speed update formula, we have:

[0034] If v i (d)<V min (d), then let v i (d) = V min (d), d = 1, 2;

[0035] If v i (d)>V max (d), then let v i (d) = V max (d), d = 1, 2.

[0036] The following nonlinear reduction strategy of inertia weight ω is adopted:

[0037]

[0038] Where, t max is the maximum allowed number of iterations G; t is the current number of iterations; ω max and ω min are the initial and final values ​​of the inertia weight, which are set to 0.95 and 0.4 respectively.

[0039] Another object of the present invention is to provide a strapdown inertial navigation system fast initial heading alignment system using the strapdown inertial navigation system fast initial heading alignment method, the strapdown inertial navigation system fast initial heading alignment system comprising:

[0040] The raw data output module is used to collect the raw data output by the inertial measurement element in the strapdown inertial navigation system in a static base state;

[0041] An initial position determination module is used to determine the longitude λ and latitude L of the initial position of the strapdown inertial navigation system using the global positioning system GPS;

[0042] The initial population construction module is used to initialize N particles in the feasible solution space to form the initial population, and randomly generate the position and speed of the particles, where N ≥ 20;

[0043] The speed calculation module is used to use the mean square error of the speed error output by the strapdown inertial navigation as the objective function, substitute the position of each particle into the inertial navigation system solution equation in turn to solve the speed, and use the root mean square of the speed error solved by the inertial navigation as the objective function to calculate the particle fitness value;

[0044] The optimal value determination module is used to compare the fitness values ​​of particles, select the current individual optimal value and group optimal value of particles with smaller fitness values.

[0045] The heading alignment module is used to update the position and velocity of the particles and return to step 4 for loop iteration until the number of iterations reaches G times. The optimal value of the particle group is the alignment result, where G ≥ 100.

[0046] Another object of the present invention is to provide a computer device, the computer device comprising a memory and a processor, the memory storing a computer program, and when the computer program is executed by the processor, the processor performs the following steps:

[0047] Initialize random particle variables, set the fitness function, substitute the particle position into the fitness function to get the fitness value, and compare the fitness values ​​of all particles to judge the quality of the particle position; in each iteration, the positions of all particles track the optimal position according to certain evolutionary rules until the iteration reaches the preset number of times; apply the PSO method to the initial heading alignment of the strapdown inertial navigation system, take the initial attitude as the optimization target of the particle swarm, and include the accelerometer zero bias in the optimization, and take the root mean square of the velocity error as the fitness function.

[0048] Another object of the present invention is to provide a computer-readable storage medium storing a computer program, wherein when the computer program is executed by a processor, the processor executes the following steps:

[0049] Initialize random particle variables, set the fitness function, substitute the particle position into the fitness function to get the fitness value, and compare the fitness values ​​of all particles to judge the quality of the particle position; in each iteration, the positions of all particles track the optimal position according to certain evolutionary rules until the iteration reaches the preset number of times; apply the PSO method to the initial heading alignment of the strapdown inertial navigation system, take the initial attitude as the optimization target of the particle swarm, and include the accelerometer zero bias in the optimization, and take the root mean square of the velocity error as the fitness function.

[0050] Another object of the present invention is to provide an information data processing terminal, which is used to implement the strapdown inertial navigation system fast initial heading alignment system.

[0051] In combination with the above technical solutions and the technical problems solved, please analyze the advantages and positive effects of the technical solutions to be protected by the present invention from the following aspects:

[0052] First, in view of the technical problems existing in the above-mentioned prior art and the difficulty of solving the problems, the technical solutions to be protected by the present invention and the results and data during the research and development process are closely combined to analyze in detail and deeply how the technical solutions of the present invention solve the technical problems, and some creative technical effects brought about after solving the problems. The specific description is as follows:

[0053] The present invention utilizes particle swarm optimization (PSO) to realize fast initial heading alignment of strapdown inertial navigation system. PSO is an optimization algorithm proposed by Kennedy and Eberhart in 1995. The PSO algorithm is designed based on the bird flock motion model. The habitat in the bird flock motion model is analogized to the position of possible solutions in the problem space to be solved. Through the transmission of information between individuals, the entire group is guided to move in the direction of possible solutions, and the possibility of finding better solutions is gradually increased in the solution process. Birds in the group are abstracted as "particles" without mass and volume. Through the mutual cooperation and information sharing between these "particles", their movement speed is affected by the historical motion state information of themselves and the group. The current movement direction and movement speed of the particles are affected by the historical optimal position of themselves and the group, and the relationship between the particles themselves and the group is better coordinated, so as to facilitate the group to perform optimization operations in a complex solution space.

[0054] The present invention relates to an initial alignment method of an inertial navigation system, and in particular to a fast initial heading alignment method of a strapdown inertial navigation system designed using a particle swarm optimization algorithm. Aiming at the initial heading alignment problem of a strapdown inertial navigation system, the present invention designs an alignment method using a PSO optimization algorithm. The present invention uses the initial attitude of the strapdown inertial navigation system and the zero bias of an accelerometer as optimization targets of a particle swarm, and uses the root mean square of a velocity error as a fitness function.

[0055] The present invention sets a fitness function for the research target by initializing a group of random particles (variables), substitutes the particle position into the fitness function to obtain the fitness value, and compares the fitness values ​​of all particles to judge the quality of the particle position. In each iteration, the position of all particles will track the optimal position (i.e., the fitness value is optimal) according to certain evolutionary rules until the iteration reaches a preset number of times. The present invention applies the PSO method to the initial heading alignment of the strapdown inertial navigation system, takes the initial attitude as the optimization target of the particle swarm, and simultaneously incorporates the accelerometer zero bias into the optimization, with the velocity error root mean square as the fitness function.

[0056] Second, considering the technical solution as a whole or from the perspective of the product, the technical effects and advantages of the technical solution to be protected by the present invention are described in detail as follows:

[0057] The technical solution of the present invention has the following advantages:

[0058] 1. Use particle swarm to directly optimize the initial attitude of strapdown inertial navigation, so there is no need to mathematically model the system and observation noise, avoiding the decrease in alignment accuracy caused by inaccurate modeling;

[0059] 2. The PSO algorithm has a fast optimization speed, which can achieve a high attitude alignment accuracy in a short time, shorten the initial heading alignment time, and improve the alignment speed;

[0060] 3. Incorporating the accelerometer zero bias into the optimization process can compensate for the alignment error caused by the accelerometer zero bias to a certain extent and improve the alignment accuracy.

[0061] Third, as auxiliary evidence of the inventiveness of the claims of the present invention, it is also reflected in the following important aspects:

[0062] (1) The expected benefits and commercial value of the technical solution of the present invention after transformation are:

[0063] The heading alignment time of the strapdown inertial navigation system can be significantly shortened, thereby speeding up the startup speed of the strapdown inertial navigation system and reducing the time cost in the application. In addition, the technical solution can ensure high heading alignment accuracy, and can still obtain high-precision heading alignment results even under the interference of complex measurement noise, thereby improving the working efficiency and performance of the strapdown inertial navigation system.

[0064] (2) The technical solution of the present invention fills the technical gap in the industry at home and abroad:

[0065] Currently, no relevant technical solutions of the present invention have been found in public documents. The application of PSO algorithm to directly align the heading of the strapdown inertial navigation system is a new breakthrough in the field of inertial navigation initial alignment technology.

[0066] (3) Whether the technical solution of the present invention solves the technical problems that people have been eager to solve but have not been able to solve successfully:

[0067] At present, high-precision and high-reliability heading alignment has always been a difficult problem that plagues the field of inertial navigation technology. The technical solution of the present invention solves this problem to a certain extent and is an important supplement to the existing heading alignment solution.

[0068] (4) Whether the technical solution of the present invention overcomes technical prejudice:

[0069] Up to now, those skilled in the art generally use Kalman filtering and other calculation methods based on estimation theory for the initial alignment of the strapdown inertial navigation system. There is a technical prejudice against the direct optimization alignment method similar to the present invention, and it is believed that the technical solution is not accurate and has a large amount of calculation. In the present invention, only the heading alignment in the alignment problem is targeted, and the heading and accelerometer zero bias are simultaneously included in the optimization, so that a higher accuracy can be obtained. At the same time, the inertia reduction and other parameter design strategies are adopted to improve the running speed of the method, which better overcomes the previous technical prejudice. BRIEF DESCRIPTION OF THE DRAWINGS

[0070] In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the following briefly introduces the drawings required for use in the embodiments of the present invention. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without creative work.

[0071] Figure 1 is a flow chart of a method for rapid initial heading alignment of a strapdown inertial navigation system provided by an embodiment of the present invention;

[0072] Figure 2 It is a structural block diagram of a fast initial heading alignment system of a strapdown inertial navigation system provided by an embodiment of the present invention;

[0073] Figure 3 is a schematic diagram of a heading alignment result provided by an embodiment of the present invention;

[0074] Figure 4 is a final heading alignment error curve diagram of test 1 provided by an embodiment of the present invention;

[0075] Figure 5 It is a heading alignment error curve diagram of the second design simulation experiment provided by an embodiment of the present invention;

[0076] Figure 6 is a graph of the final experimental three-direction alignment error provided by an embodiment of the present invention;

[0077] In the figure: 1. Raw data output module; 2. Initial position determination module; 3. Initial population construction module; 4. Speed ​​solution module; 5. Optimal value determination module; 6. Heading alignment module. DETAILED DESCRIPTION

[0078] In order to make the purpose, technical solution and advantages of the present invention more clearly understood, the present invention is further described in detail below in conjunction with the embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not used to limit the present invention.

[0079] In view of the problems existing in the prior art, the present invention provides a method, system and terminal for fast initial heading alignment of a strapdown inertial navigation system. The present invention is described in detail below in conjunction with the accompanying drawings.

[0080] 1. Explanatory Examples In order to enable those skilled in the art to fully understand how to implement the present invention, this section provides an illustrative example that expands and describes the technical solution of the claims.

[0081] Explanation of terms: (1) Inertial navigation system: An inertial navigation system (INS) is an autonomous navigation system that does not rely on external information and does not radiate energy to the outside. Its basic working principle is based on Newton's laws of mechanics. By measuring the acceleration of the carrier in the inertial reference system, integrating it over time, and transforming it into the navigation coordinate system, it is possible to obtain information such as velocity, attitude, and position in the navigation coordinate system. (2) Strapdown inertial navigation system: An inertial navigation system that directly attaches a gyroscope and an accelerometer to a carrier is called a strapdown inertial navigation system (INS). The basic principle is to use the angular velocity output by the gyroscope to calculate the real-time carrier attitude, and to convert the absolute acceleration measured by the accelerometer into the navigation coordinate system through mathematical operations for solution. (3) Initial alignment: INS is an integral and dead reckoning system that requires the output of the inertial measurement unit (including the gyroscope and the accelerometer) to be integrated and solved. Therefore, before the INS enters the navigation solution, it is necessary to determine the initial value of the integral, that is, to perform initial alignment on it. Initial alignment is a key step to ensure the normal operation of the inertial navigation system. Any initial alignment error will enter the integral calculation and cause system error. (4) Heading alignment: The initial alignment of the strapdown inertial navigation system requires determining the initial position, attitude and heading information of the vehicle. The position information is easy to obtain through the external reference system, and the attitude information can be calculated through the accelerometer measurement value, while the heading information is difficult to obtain. The process of determining the heading of the strapdown inertial navigation system during the initial alignment is called heading alignment.

[0082] like Figure 1 As shown, the strapdown inertial navigation system fast initial heading alignment method provided by the embodiment of the present invention comprises the following steps:

[0083] S101, collecting raw data output by an inertial measurement element in a strapdown inertial navigation system in a static base state;

[0084] S102, determining the longitude and latitude of the initial position of the strapdown inertial navigation system using a global positioning system (GPS);

[0085] S103, initializing N particles in the feasible solution space to form an initial population, and randomly generating positions and velocities of the particles;

[0086] S104, taking the mean square error of the speed error output by the strapdown inertial navigation system as the objective function, substituting the position of each particle into the inertial navigation system solution equation in turn to solve the speed, and calculating the particle fitness value using the mean square root of the speed error solved by the inertial navigation system as the objective function;

[0087] S105, comparing the fitness values ​​of the particles, selecting the current individual optimal value and the group optimal value of the particle with the smaller fitness value as the better one;

[0088] S106, update the position and velocity of the particles, and return to S104 for loop iteration until the number of iterations reaches G times, at which time the optimal value of the particle group is the alignment result.

[0089] like Figure 2 As shown, the strapdown inertial navigation system fast initial heading alignment system provided by the embodiment of the present invention comprises:

[0090] The raw data output module 1 is used to collect the raw data output by the inertial measurement element in the strapdown inertial navigation system in a static base state;

[0091] The initial position determination module 2 is used to determine the longitude λ and latitude L of the initial position of the strapdown inertial navigation system using the global positioning system GPS;

[0092] The initial population construction module 3 is used to initialize N particles in the feasible solution space to form an initial population, and randomly generate the position and speed of the particles, where N ≥ 20;

[0093] The speed calculation module 4 is used to use the mean square error of the speed error output by the strapdown inertial navigation as the objective function, substitute the position of each particle into the inertial navigation system solution equation in turn to solve the speed, and use the root mean square of the speed error solved by the inertial navigation as the objective function to calculate the particle fitness value;

[0094] The optimal value determination module 5 is used to compare the fitness values ​​of the particles, select the current individual optimal value and the group optimal value of the particles with the smaller fitness value as the better one;

[0095] The heading alignment module 6 is used to update the position and speed of the particles, and return to step 4 to perform loop iteration until the number of iterations reaches G times. The optimal value of the particle group is the alignment result, where G≥100.

[0096] Example 1

[0097] The present invention aims at the initial heading alignment problem of the strapdown inertial navigation system and designs an alignment method using the PSO optimization algorithm. The initial attitude of the strapdown inertial navigation system and the accelerometer zero bias are used as the optimization targets of the particle swarm, and the velocity error root mean square is used as the fitness function. The specific technical scheme is as follows:

[0098] Step 1: Under a static base state, collect raw data output by an inertial measurement element (including a gyroscope and an accelerometer) in a strapdown inertial navigation system;

[0099] Step 2: using the global positioning system GPS to determine the longitude λ and latitude L of the initial position of the strapdown inertial navigation system;

[0100] Step 3: Initialize N (N≥20) particles in the feasible solution space to form an initial population, and randomly generate the positions and velocities of the particles;

[0101] Step 4: Taking the mean square error of the speed error output by the strapdown inertial navigation as the objective function, the position of each particle is substituted into the inertial navigation system solution equation to solve the speed, and the speed error root mean square solved by the inertial navigation is used as the objective function to calculate the particle fitness value;

[0102] Step 5: Compare the fitness values ​​of the particles, select the current individual optimal value and group optimal value of the particle with the smaller fitness value.

[0103] Step 6: Update the position and velocity of the particles and return to step 4 for loop iteration until the number of iterations reaches G (G ≥ 100). At this time, the optimal value of the particle group is the alignment result.

[0104] In the step 1, the sampling time of the raw data of the inertial measurement element is set to 100s, and the sampling frequency is set to 1Hz, where s is a time unit, representing "seconds"; Hz is a frequency unit, representing "times / second".

[0105] In step 3, the current position of the i-th (i=1, 2, ..., N) particle is recorded as x i =[x i1 ,x i2 ], where x i1 represents the inertial navigation direction, x i2 represents the accelerometer zero bias, and the particle flying speed is v i =[v i1 ,v i2 ].

[0106] In step 4, the x in each particle position is i1 As the heading value, x i2 Substitute the accelerometer zero bias value into the inertial navigation solution equation to obtain the velocity v′ calculated by the inertial navigation solution. n (n=1,2,...,100). v′ n =[v′ nE ,v′ nN ], where v′ nE , v′ nNThey represent the eastward speed and northward speed calculated by the inertial navigation respectively. The objective function is designed as:

[0107]

[0108] The calculation result of the above formula is the fitness value of each particle.

[0109] In step 5, after iteration, let the optimal solution currently found by each particle be p i =[p i1 ,p i2 ], the global optimal solution currently found by the population is p g =[p g1 ,p g2 ].

[0110] In step 6, the particle position and velocity update equations are:

[0111] x i+1 =x i +v i

[0112] v i+1 =ωv i +c1R1(p i -x i )+c2R2(p g -x i )

[0113] Where: c1 and c2 are usually called acceleration coefficients (or learning factors). These two coefficients enable particles to learn by themselves and learn from excellent individuals in the population. Their values ​​are 2; R1 and R2 are random numbers uniformly distributed between [0,1] and are independent of each other. ω is the inertia weight, which determines the influence of the previous velocity of the particle on the current velocity and plays a role in balancing the global search and local search capabilities of the algorithm.

[0114] The particle velocity is limited to [V max ,V min ], set V max =[0.1rad,1ug], V min =-[0.1rad,1ug], where rad is the heading angle unit, representing "radians"; ug is the accelerometer zero bias unit, 1ug ​​= 1×10 -6 g, g represents the acceleration due to gravity, which is taken as 9.78m / s in this scheme 2 , where m / s 2 is the unit of acceleration, representing "meters per second squared". After executing the velocity update formula, we have:

[0115] If v i (d)<Vmin (d), then let v i (d) = V min (d), d = 1, 2;

[0116] If v i (d)>V max (d), then let v i (d) = V max (d), d = 1, 2.

[0117] The nonlinear decreasing strategy of inertia weight ω can make the algorithm have stronger exploration ability in the early stage of iteration, and can continuously search new areas, and then the convergence ability is gradually enhanced, prompting the algorithm to conduct a fine search near possible solutions. This scheme adopts the following decreasing strategy:

[0118]

[0119] Where: t max is the maximum allowed number of iterations G; t is the current number of iterations; ω max and ω min are the initial and final values ​​of the inertia weight, which are set to 0.95 and 0.4 respectively in this scheme.

[0120] Example 2

[0121] The strapdown inertial navigation system fast initial heading alignment method provided by the embodiment of the present invention comprises:

[0122] Step 1: Under the static base state, collect the raw data output by the inertial measurement element (including gyroscope and accelerometer) in the strapdown inertial navigation system. In the embodiment, the simulation data is used for the test. The basic parameters of the simulated inertial navigation are: the gyroscope constant drift is 0.01° / h. The constant bias of the accelerometer is 100ug, where ° / h is the gyroscope drift unit, representing "degrees per hour"; ug is the accelerometer zero bias unit, 1ug=1×10 -6 g, g represents the acceleration due to gravity, which is taken as 9.78m / s in this scheme 2 , where m / s 2 It is a unit of acceleration, representing "meters per second squared".

[0123] Step 2: Use the global positioning system GPS to determine the longitude λ and latitude L of the initial position of the strapdown inertial navigation system. In the embodiment, the longitude and latitude are set to 120 degrees east longitude and 30 degrees north latitude respectively.

[0124] Step 3: Initialize 30 particles in the feasible solution space to form an initial population, and randomly generate the positions and velocities of the particles.

[0125] Step 4, taking the mean square error of the speed error output by strapdown inertial navigation as the objective function, the position of each particle is substituted into the inertial navigation system solution equation to perform speed solution in turn, and the speed error root mean square of the inertial navigation solution is used as the objective function to calculate the particle fitness value. In an embodiment, the initial true attitude of the strapdown inertial navigation is set to: pitch angle 0 degree, roll angle 0 degree, and heading angle 60 degree. The gyro and accelerometer data under the static base are generated by simulation, assuming that the strapdown inertial navigation pitch angle and roll angle have been obtained by other methods, and attitude and speed solution are performed after being brought into the particle position to calculate the fitness value.

[0126] Step 5: Compare the fitness values ​​of the particles, select the current individual optimal value and group optimal value of the particle based on the smaller fitness value.

[0127] Step 6: Update the position and velocity of the particle, and return to step 4 for loop iteration until the number of iterations reaches 200. At this time, the optimal value of the particle group is the alignment result. Record the optimal value of the particle group in each iteration, and use the heading angle in the particle optimal value to subtract the true heading angle to obtain the heading alignment error value. The heading alignment error curve is shown in Figure 3 shown.

[0128] Figure 3 In the embodiment, the heading alignment result obtained by applying the scheme of the present invention uses 100s of strapdown inertial navigation raw data. When the iteration is performed for 100 times, the alignment error has converged well. After 200 iterations, the final heading angle alignment error is about 7 arc minutes.

[0129] Example 3

[0130] The strapdown inertial navigation system fast initial heading alignment method provided by the embodiment of the present invention comprises:

[0131] Step 1: Under the static base state, collect the raw data output by the inertial measurement element (including gyroscope and accelerometer) in the strapdown inertial navigation system. In the embodiment, the simulation data is used for the test. The basic parameters of the simulated inertial navigation are: the gyroscope constant drift is 0.01° / h. The constant bias of the accelerometer is 100ug, where ° / h is the gyroscope drift unit, representing "degrees per hour"; ug is the accelerometer zero bias unit, 1ug=1×10 -6 g, g represents the acceleration due to gravity, which is taken as 9.78m / s in this scheme 2 , where m / s 2 It is a unit of acceleration, representing "meters per second squared".

[0132] Step 2: Use the global positioning system GPS to determine the longitude λ and latitude L of the initial position of the strapdown inertial navigation system. In the embodiment, the longitude and latitude are set to 120 degrees east longitude and 30 degrees north latitude respectively.

[0133] Step 3: Initialize 30 particles in the feasible solution space to form an initial population, and randomly generate the positions and velocities of the particles.

[0134] Step 4, taking the mean square error of the speed error output by strapdown inertial navigation as the objective function, the position of each particle is substituted into the inertial navigation system solution equation to perform speed solution in turn, and the speed error root mean square of the inertial navigation solution is used as the objective function to calculate the particle fitness value. In an embodiment, the initial true attitude of the strapdown inertial navigation is set to: pitch angle 0 degree, roll angle 0 degree, and heading angle 60 degree. The gyro and accelerometer data under the static base are generated by simulation, assuming that the strapdown inertial navigation pitch angle and roll angle have been obtained by other methods, and attitude and speed solution are performed after being brought into the particle position to calculate the fitness value.

[0135] Step 5: Compare the fitness values ​​of the particles, select the current individual optimal value and group optimal value of the particle based on the smaller fitness value.

[0136] Step 6: Update the position and velocity of the particle, and return to step 4 for loop iteration until the number of iterations reaches 200. At this time, the optimal value of the particle group is the alignment result. Record the optimal value of the particle group in each iteration, and use the heading angle in the particle optimal value to subtract the true heading angle to obtain the heading alignment error value. The heading alignment error curve is shown in Figure 3 shown.

[0137] Figure 3 In the embodiment, the heading alignment result obtained by applying the scheme of the present invention uses 100s of strapdown inertial navigation raw data. When the iteration is performed for 100 times, the alignment error has converged well. After 200 iterations, the final heading angle alignment error is about 7 arc minutes.

[0138] It should be noted that the embodiments of the present invention can be implemented by hardware, software, or a combination of software and hardware. The hardware part can be implemented using dedicated logic; the software part can be stored in a memory and executed by an appropriate instruction execution system, such as a microprocessor or dedicated design hardware. A person of ordinary skill in the art will appreciate that the above-mentioned devices and methods can be implemented using computer executable instructions and / or contained in a processor control code, such as a carrier medium such as a disk, CD or DVD-ROM, a programmable memory such as a read-only memory (firmware), or a data carrier such as an optical or electronic signal carrier. Such code is provided on the carrier medium. The device and its modules of the present invention can be implemented by hardware circuits such as very large-scale integrated circuits or gate arrays, semiconductors such as logic chips, transistors, etc., or programmable hardware devices such as field programmable gate arrays, programmable logic devices, etc., can also be implemented by software executed by various types of processors, and can also be implemented by a combination of the above-mentioned hardware circuits and software, such as firmware.

[0139] 2. Evidence of the effects of the embodiments The embodiments of the present invention have achieved some positive effects during the development or use process, and indeed have great advantages over the prior art. The following content is described in conjunction with the data, charts, etc. of the test process.

[0140] Design simulation test 1: The gyro constant drift is 0.02° / h, and the accelerometer constant bias is 200ug, where ° / h is the gyro drift unit, representing "degrees per hour"; ug is the accelerometer zero bias unit, 1ug ​​= 1×10 -6 g, g represents the acceleration due to gravity, which is taken as 9.78m / s in this scheme 2 , where m / s 2 is the unit of acceleration, representing "meters per second squared". Assume that the longitude and latitude are 110 degrees east longitude and 40 degrees north latitude respectively, and set the initial true attitude of the strapdown inertial navigation to: pitch angle 0 degrees, roll angle 0 degrees, heading angle 30 degrees. Collect 100s of strapdown inertial navigation raw data, and perform 200 iterations in total. The final heading alignment error curve of Experiment 1 is as follows: Figure 4 shown.

[0141] Design simulation experiment 2: Compared with experiment 1, other conditions remain unchanged and the constant bias of the accelerometer is set to 50ug. The final heading alignment error curve is as follows: Figure 5 shown.

[0142] Design simulation experiment 3: Compared with experiment 1, other conditions remain unchanged, and the constant bias of the accelerometer is set to 10ug. The final heading alignment error curve of experiment 3 is as follows Figure 6 shown.

[0143] The heading alignment errors of the three tests were 10 arc minutes, 8 arc minutes, and 5 arc minutes, respectively, which proves that the technical solution of the present invention can achieve better results.

[0144] The above description is only a specific implementation mode of the present invention, but the protection scope of the present invention is not limited thereto. Any modifications, equivalent substitutions and improvements made by any technician familiar with the technical field within the technical scope disclosed by the present invention and within the spirit and principle of the present invention should be covered by the protection scope of the present invention.

Claims

1. A method for fast initial heading alignment of a strapdown inertial navigation system, characterized in that: The strapdown inertial navigation system fast initial heading alignment method comprises: Initialize random particle variables, set fitness function, substitute particle position into fitness function to get fitness value, and compare the fitness values ​​of all particles to judge the quality of particle position; in each iteration, the position of all particles tracks the optimal position according to certain evolution rules until the iteration reaches the preset number of times; apply PSO method to the initial heading alignment of strapdown inertial navigation system, take initial attitude as the optimization target of particle swarm, and include accelerometer zero bias in optimization, and take velocity error root mean square as fitness function; The strapdown inertial navigation system fast initial heading alignment method comprises the following steps: Step 1: Under a static base state, collect raw data output by an inertial measurement element in a strapdown inertial navigation system; Step 2: using the global positioning system GPS to determine the longitude λ and latitude L of the initial position of the strapdown inertial navigation system; Step 3: Initialize N particles in the feasible solution space to form an initial population, and randomly generate the position and velocity of the particles, where N ≥ 20; Step 4: Taking the mean square error of the speed error output by the strapdown inertial navigation as the objective function, the position of each particle is substituted into the inertial navigation system solution equation to solve the speed, and the speed error root mean square solved by the inertial navigation is used as the objective function to calculate the particle fitness value; Step 5: Compare the fitness values ​​of the particles, select the current individual optimal value and group optimal value of the particle with the smaller fitness value. Step 6: Update the position and velocity of the particles and return to step 4 for loop iteration until the number of iterations reaches G. At this time, the optimal value of the particle group is the alignment result, where G ≥ 100.

2. The strapdown inertial navigation system rapid initial heading alignment method as claimed in claim 1, characterized in that: The inertial measurement element in step 1 includes a gyroscope and an accelerometer; the sampling time of the raw data of the inertial measurement element is set to 100s, and the sampling frequency is set to 1Hz; In step 3, the current position of the i-th particle, i=1, 2, ..., N, is x i =[x i1 ,x i2 ], where x i1 represents the inertial navigation direction, x i2 represents the accelerometer zero bias, and the particle flying speed is v i =[v i1 ,v i2 ].

3. The strapdown inertial navigation system rapid initial heading alignment method as claimed in claim 1, characterized in that: In step 4, the x in each particle position is i1 As the heading value, x i2 Substitute the accelerometer zero bias value into the inertial navigation solution equation to obtain the velocity v′ calculated by the inertial navigation solution. n , n=1,2,...,100; v′ n =[v′ nE ,v′ nN ], where v′ nE , v′ nN They represent the eastward speed and northward speed calculated by the inertial navigation respectively; The objective function is designed as: The calculation result is the fitness value of each particle.

4. The strapdown inertial navigation system rapid initial heading alignment method as claimed in claim 1, characterized in that: In step 5, after iteration, let the optimal solution currently found by each particle be p i =[p i1 ,p i2 ], the global optimal solution currently found by the population is p g =[p g1 ,p g2 ].

5. The strapdown inertial navigation system rapid initial heading alignment method as claimed in claim 1, characterized in that: In step 6, the particle position and velocity update equations are: x i+1 =x i +v i ; v i+1 =ωv i +c1R1(p i -x i )+c2R2(p g -x i ); Where c1 and c2 are acceleration coefficients or learning factors, which are used to enable particles to learn by themselves and learn from excellent individuals in the population, and their values ​​are 2; R1 and R2 are random numbers uniformly distributed between [0,1] and are independent of each other; ω is the inertia weight, which is used to determine the degree of influence of the particle's previous speed on the current speed and balance the algorithm's global search and local search capabilities; The particle velocity is limited to [V max ,V min ], set V max =[0.1rad,1ug],V min =-[0.1rad,1ug], where rad is the heading angle unit, representing "radians"; ug is the accelerometer zero bias unit, 1ug ​​= 1×10 -6 g, g represents the acceleration due to gravity, which is 9.78m / s 2 ; After executing the speed update formula, we have: If v i (d)<V min (d), then let v i (d) = V min (d), d = 1, 2; If v i (d)>V max (d), then let v i (d) = V max (d), d = 1, 2; The following nonlinear reduction strategy of inertia weight ω is adopted: In the formula, t max is the maximum allowed number of iterations G; t is the current number of iterations; ω max and ω min are the initial and final values ​​of the inertia weight, which are set to 0.95 and 0.4 respectively.

6. A strapdown inertial navigation system rapid initial heading alignment system using the strapdown inertial navigation system rapid initial heading alignment method according to any one of claims 1 to 5, characterized in that: The strapdown inertial navigation system fast initial heading alignment system comprises: The raw data output module is used to collect the raw data output by the inertial measurement element in the strapdown inertial navigation system in a static base state; An initial position determination module is used to determine the longitude λ and latitude L of the initial position of the strapdown inertial navigation system using the global positioning system GPS; The initial population construction module is used to initialize N particles in the feasible solution space to form the initial population and randomly generate the position and velocity of the particles, where N ≥ 20; The speed calculation module is used to use the mean square error of the speed error output by the strapdown inertial navigation as the objective function, substitute the position of each particle into the inertial navigation system solution equation in turn to solve the speed, and use the root mean square of the speed error solved by the inertial navigation as the objective function to calculate the particle fitness value; The optimal value determination module is used to compare the fitness values ​​of particles, select the current individual optimal value and group optimal value of particles with smaller fitness values. The heading alignment module is used to update the position and velocity of the particles and return to step 4 for loop iteration until the number of iterations reaches G times. The optimal value of the particle group is the alignment result, where G ≥ 100.

7. A computer device, characterized in that: The computer device includes a memory and a processor, the memory stores a computer program, and when the computer program is executed by the processor, the processor executes the steps of the fast initial heading alignment method of the strapdown inertial navigation system as claimed in any one of claims 1 to 5.

8. A computer-readable storage medium storing a computer program, wherein when the computer program is executed by a processor, the processor executes the steps of the strapdown inertial navigation system fast initial heading alignment method according to any one of claims 1 to 5.

9. An information data processing terminal, characterized in that: The information data processing terminal is used to implement the strapdown inertial navigation system rapid initial heading alignment system as described in claim 6.

Images