Geomagnetic matching navigation algorithm based on improved ant colony algorithm

Abstract

The invention discloses a geomagnetic matching navigation algorithm based on an improved ant colony algorithm, and belongs to the field of geomagnetic navigation, control and guidance. The algorithm is implemented by the following steps: acquiring initial position information, detecting a magnetic measurement value of data by a geomagnetic sensor, and resolving a magnetic course angle; initializing parameters of the ant colony algorithm; putting the ant at an initial position, and transferring the ant to a next node according to a state transition probability formula until the ant reaches a magnetic measurement sequence; calculating and recording a global similarity measurement function of each ant, and updating pheromones; when the maximum number of iterations is reached, the optimal ant path is output, and the optimal geomagnetic navigation trajectory can be obtained. According to the method, approaching to the optimal solution is realized by utilizing the search randomness and positive feedback behavior of the ant colony algorithm, the optimal track is obtained through the algorithm, and the improvement of the geomagnetic navigation matching precision is realized.

Description

Technical Field

[0001] The invention belongs to the field of geomagnetic matching navigation, and in particular relates to a geomagnetic matching navigation algorithm based on an improved ant colony algorithm. Background Art

[0002] Navigation technology has a long history and has developed rapidly. It is an engineering technology that crosses multiple disciplines. With the development of the economy, the advancement of science and technology, and the development of travel tools, the demand for navigation systems is increasing. The development of key scientific and technological fields such as computers, electronics, and signal processing has also enabled people to use electronic, optical, and magnetic methods to determine the parameters related to the positioning of moving bodies to locate and navigate moving bodies. The development of navigation technology has shown a diversified trend, and people's demand for navigation technology has also increased.

[0003] At present, navigation systems mainly include inertial navigation, geomagnetic matching navigation, celestial navigation, gravity matching navigation, satellite navigation, etc. Although satellite navigation has high positioning accuracy, the signal is weak and easily interfered with, and it is difficult to receive the signal in some environments; terrain matching navigation is conducive to combined navigation research in places with large terrain fluctuations, but navigation work is limited in places such as deserts and seas where terrain changes are not obvious; although celestial navigation can provide accurate position and attitude information, it also has the defects of discontinuous transmission signals and being easily affected by climatic conditions. Geomagnetic navigation can make up for the shortcomings of existing navigation to a certain extent and is a more promising navigation technology.

[0004] In recent years, driven by relevant researchers, geomagnetic matching navigation technology has made great achievements. It has been widely used in many fields such as aircraft control, prospecting, earthquake monitoring and prediction, missile guidance, anti-submarine, underground positioning, underwater carrier positioning, etc. In addition, the investment cost of geomagnetic navigation equipment is lower than that of inertial navigation, so it has important military value. The geomagnetic field is an inherent resource of the earth. The vector magnetic field of each point on the earth corresponds one-to-one with the longitude and latitude and the height above the ground. At the same time, the process of detecting the magnetic field value is a passive reception of information, and no signal is emitted outward, which has good concealment. Geomagnetic matching is to measure the local magnetic field strength with a magnetometer on the route of the object, compare it with the pre-stored geomagnetic map, and then match the actual track position.

[0005] Although geomagnetic matching navigation technology has many advantages, it still faces some challenges in practical applications. First, the measurement accuracy and stability of the geomagnetic field directly affect the accuracy of navigation, so high-precision geomagnetic sensors are required. Second, the construction and update of geomagnetic maps is also a complex process that requires a lot of field measurements and data processing. In addition, local anomalies and interferences in the geomagnetic field (such as magnetic minerals, artificial magnetic fields, etc.) may also affect the accuracy of navigation. The traditional ant colony-based geomagnetic matching algorithm mainly relies on the total intensity of the geomagnetic field for matching, and fails to make full use of the vector information of the geomagnetic field, resulting in limited matching accuracy. In addition, the same pheromone concentration in the initial iteration leads to uncertain path selection, and only relies on the total intensity of the geomagnetic field for matching. The motion trajectory has a large uncertainty. The previous ant movement path will interfere with the subsequent solution. The suboptimal solution may prevail in the search process. In addition, the pheromone of the traditional algorithm is distributed on the path between the grid nodes, which is not conducive to storage and calculation. Summary of the invention

[0006] In order to solve the problems of large errors and inaccurate positioning in traditional geomagnetic matching algorithms, the present invention proposes a geomagnetic matching navigation algorithm based on an improved ant colony algorithm. By introducing heuristic functions, weighted state transition probability of magnetic heading angle, and pheromone update of the three components of the geomagnetic field, the geomagnetic matching algorithm is optimized to effectively improve navigation accuracy and efficiency, showing significant advantages, especially in complex environments.

[0007] To achieve the above object, the technical solution adopted by the present invention is:

[0008] The geomagnetic matching navigation method based on the improved ant colony algorithm includes the following steps:

[0009] (1) The geomagnetic information is collected in real time through a three-axis geomagnetic sensor module and an accelerometer module.

[0010] (2) The magnetic heading angle of each point is calculated based on the collected information, and a discrete geomagnetic database is established together with the three-axis geomagnetic data.

[0011] (3) Initialize the parameters of the ant colony optimization algorithm, including the pheromone volatilization factor , maximum number of iterations , the intensity factor Q of the pheromone increment, the pheromone importance factor , heuristic function importance factor , directional weight factor , the number of ants m, the geomagnetic map, and the pheromone matrix at the same latitude as the geomagnetic map.

[0012] (4) Start the iteration, place m ants at the starting point, calculate the heuristic function based on pheromone and the data from the geomagnetic database, calculate the state transition probability based on the heading angle weighted function, and select the next node for each ant until the measurement sequence length is reached.

[0013] (5) Calculate the global similarity measurement function of each ant , and calculate the fitness of each ant , and record the fitness of the best ant among all ants in each iteration , and the average of the fitness functions of all ants .

[0014] (6) Calculate the pheromone concentration, update the pheromone paths whose fitness is better than the mean, and set upper and lower limits for pheromones and pheromone increments.

[0015] (7) Determine whether the maximum number of iterations has been reached. If not, proceed to step 4.

[0016] (8) The ant path with the maximum fitness function is selected as the final matching path of the algorithm in the iterations. The index of the ant in the pheromone matrix is ​​substituted into the geomagnetic map grid, which can be converted into longitude and latitude as the output matching result.

[0017] As a further improvement of the present invention, the specific steps of obtaining magnetic heading information in step (2) are as follows:

[0018] The geomagnetic field is a vector. For a fixed location, this vector can be decomposed into two components parallel to the local horizontal plane and one component perpendicular to the local horizontal plane. When the compass is in a horizontal state, the three-axis data measured are the geomagnetic components of the machine system. For the two horizontal components, their vector sum always points to magnetic north.

[0019] When the multi-rotor drone is in any posture, the measurement output of the accelerometer in the body coordinate system is:

[0020] ,

[0021] in, represents the acceleration vector, They represent the components of acceleration on the x, y, and z axes in the body coordinate system.

[0022] The magnetic field strength of the x, y, and z axes output by the magnetometer in the body coordinate system is ;

[0023] Correct the tilt angle and convert the data of the body coordinate system into a horizontal body coordinate system:

[0024] ,

[0025] ,

[0026] in, , are the magnetic field components of the x and y axes in the horizontal body coordinate system.

[0027] Use the same method to find the angle between the x-axis and magnetic north:

[0028] ,

[0029] The heading angle calculated by the magnetometer is the x-axis of the body coordinate relative to the magnetic north, and there is a magnetic declination between the true north and the magnetic north. , so the heading angle of the body coordinate longitudinal axis relative to true north is:

[0030] ,

[0031] As a further improvement of the present invention, the specific steps of the ant search obtained in step (4) are as follows:

[0032] When solving at level t, the formula for calculating the state probability of transferring from point i on the grid to point j is as follows:

[0033] ,

[0034] Among them, S is the set of locations that ant k can reach in one step when it is at point i, and m is the number of ants passing through this path. is the pheromone intensity of point j when solving at level t ( Similarly, it represents the pheromone on the set s, and the others can be explained by analogy with the following variables). is the heuristic function, as shown below:

[0035] ,

[0036] in, is the magnetic field strength at point j on the geomagnetic map, It represents the magnetic field strength of the t-th point in the magnetic measurement sequence. The corresponding position is point j on the magnetic map, and point j is the one-step arrival position of point i. is the heading angle weighting function, as shown below:

[0037] ,

[0038] in, is the magnetic heading angle of point j relative to point i on the geomagnetic map, with north-east as positive. It is the magnetic heading angle obtained by the sensor in real time.

[0039] As a further improvement of the present invention, the metric function of the ant in step (5) is updated as follows:

[0040] The geomagnetic vector is used to calculate the global similarity measurement function. The global similarity measurement function of the kth ant is shown as follows:

[0041] ,

[0042] In the formula, is the global similarity measurement function, N is the matching length, It represents the total magnetic field intensity value on the magnetic map at step t with p as the starting point. , , Indicates the north component, east component, and longitudinal component of the corresponding position on the geomagnetic map. represents the total intensity measured by the magnetic sensor, , , They respectively represent the north component, east component, and longitudinal component obtained by the magnetic sensor.

[0043] The fitness of the kth ant is:

[0044] ,

[0045] As a further improvement of the present invention, the specific process of updating pheromones in step (6) is as follows:

[0046] ,

[0047] ,

[0048] so:

[0049] ,

[0050] In the formula, is the pheromone increment of the kth ant, is the total pheromone increment of each iterative path, Q is the intensity factor of the pheromone increment, m is the total number of ants, is the number of iterations, It is a pheromone volatile factor. Indicates the lower limit of pheromone concentration, Indicates the upper limit of pheromone concentration, is the pheromone variable.

[0051] As a further improvement of the present invention, the specific process of converting longitude and latitude in step (8) is as follows:

[0052] Find the ant with the largest fitness function among all iterations:

[0053] ,

[0054] According to the above, the ant with the largest fitness function is selected, and the index of the ant in the pheromone matrix is ​​substituted into the grid of the geomagnetic map, which can be converted into longitude and latitude as the output matching result.

[0055] The beneficial effects of the present invention are:

[0056] The present invention effectively solves the problems of the same pheromone concentration in the initial iteration of the traditional ant colony geomagnetic matching algorithm, which leads to uncertainty in path selection, and only relies on the total strength of the geomagnetic field for matching, which leads to local optimality and low navigation accuracy. When the ant selects the next node, the heuristic function and the magnetic heading angle weighting function are introduced. The local heuristic function adopts a single-point MAD algorithm to calculate the difference between the magnetic measurement value and the magnetic field value of the corresponding point on the magnetic map, and uses its reciprocal as the local heuristic function. The magnetic heading angle is calculated and obtained by a three-axis magnetometer, thereby optimizing the ant's selection of the optimal path at a certain local point.

[0057] The size of pheromone is updated through the three components of the geomagnetic field, making full use of the characteristics of the geomagnetic vector.

[0058] According to the characteristics of geomagnetic matching and geomagnetic map, pheromones are distributed on each node of each geomagnetic map grid. The size of pheromone at each node is used to determine the degree of attraction to ants, effectively reducing the spatial complexity of the algorithm. BRIEF DESCRIPTION OF THE DRAWINGS

[0059] Figure 1 This is a flow chart of a geomagnetic matching navigation algorithm based on an improved ant colony algorithm of the present invention;

[0060] Figure 2 A matching result diagram of a geomagnetic matching navigation algorithm based on an improved ant colony algorithm according to the present invention;

[0061] Figure 3 This is an error diagram of a geomagnetic matching navigation algorithm based on an improved ant colony algorithm according to the present invention. DETAILED DESCRIPTION

[0062] The present invention is further described in detail below in conjunction with the accompanying drawings and specific embodiments:

[0063] A geomagnetic matching navigation algorithm based on an improved ant colony algorithm comprises the following steps:

[0064] Step 1: Collect geomagnetic information in real time through a three-axis geomagnetic sensor module and an accelerometer module.

[0065] Step 2: Calculate the magnetic heading angle of each point based on the collected information, and build a discrete geomagnetic database together with the three-axis geomagnetic data. The specific steps for calculating the heading angle are as follows:

[0066] When the multi-rotor drone is in any posture, the measurement output of the accelerometer in the body coordinate system is:

[0067] ,

[0068] in, represents the acceleration vector, Respectively represent the components of acceleration on the x, y, and z axes in the body coordinate system;

[0069] ,

[0070] ,

[0071] in, , Pitch angle and roll angle respectively;

[0072] The magnetic field strength of the x, y, and z axes output by the magnetometer in the body coordinate system is ;

[0073] Correct the tilt angle and convert the data of the body coordinate system into a horizontal body coordinate system:

[0074] ,

[0075] ,

[0076] in, , are the magnetic field components of the x and y axes in the horizontal body coordinate system.

[0077] Use the same method to find the angle between the x-axis and magnetic north:

[0078] ,

[0079] There is a magnetic declination between true north and magnetic north: , so the heading angle of the body coordinate longitudinal axis relative to true north is:

[0080] ,

[0081] Step 3: Initialize the parameters of the ant colony optimization algorithm, including the pheromone volatilization factor , maximum number of iterations , the intensity factor Q of the pheromone increment, the pheromone importance factor , heuristic function importance factor , directional weight factor , the number of ants m, the geomagnetic map, and the pheromone matrix at the same latitude as the geomagnetic map;

[0082] Step 4, start iteration, place m ants at the starting point, calculate the heuristic function based on pheromone and the data of the geomagnetic library, calculate the state transition probability based on the heading angle weighted function, and select the next node for each ant until the measurement sequence length is reached;

[0083] In geomagnetic matching, since the geomagnetic map is in grid form, the feasible solution for each movement of the ant is a point on the geomagnetic grid map. All the paths taken by each ant are the path results of geomagnetic matching. Therefore, geomagnetic matching can be regarded as a multi-level discrete optimization problem. Assuming that the magnetic measurement sequence is n and the size of the geomagnetic map is M N, then the solution is n levels, each level has M N nodes are optional. When solving at level t, the calculation formula for the state probability of transferring from point i on the grid graph to point j is as follows:

[0084] ,

[0085] Among them, S is the set of locations that ant k can reach in one step when it is at point i, and m is the number of ants passing through this path. is the pheromone intensity of point j when solving at level t, is the heuristic function, as shown below:

[0086] ,

[0087] in, is the magnetic field strength at point j on the geomagnetic map, It represents the magnetic field strength of the t-th point in the magnetic measurement sequence. The corresponding position is point j on the magnetic map, and point j is the one-step arrival position of point i. is the heading angle weighting function, as shown below:

[0088] ,

[0089] in, is the magnetic heading angle of point j relative to point i on the geomagnetic map, with north-east as positive. It is the magnetic heading angle obtained by the sensor in real time.

[0090] Step 5: Calculate the global similarity metric function of each ant , and calculate the fitness of each ant , and record the optimal sequence , and the average value of the fitness function .

[0091] The geomagnetic vector is used to calculate the global similarity measurement function. The global similarity measurement function of the kth ant is shown as follows:

[0092] ,

[0093] In the formula, is the global similarity metric function of the kth ant, N is the matching length, It represents the total magnetic field intensity value on the magnetic map at step t with p as the starting point. , , Indicates the north component, east component, and longitudinal component of the corresponding position on the geomagnetic map. represents the total intensity measured by the magnetic sensor, , , They respectively represent the north component, east component, and longitudinal component obtained by the magnetic sensor.

[0094] The fitness of the kth ant is:

[0095] ,

[0096] Step 6: Calculate the pheromone concentration, update the pheromone paths whose fitness is better than the mean, and set upper and lower limits for pheromones and pheromone increments.

[0097] ,

[0098] ,

[0099] so:

[0100] ,

[0101] In the formula, is the pheromone increment of the kth ant, is the total pheromone increment of each iterative path, Q is the intensity factor of the pheromone increment, m is the total number of ants, NC is the number of iterations, It is a pheromone volatile factor. Indicates the lower limit of pheromone concentration, Indicates the upper limit of pheromone concentration, is the pheromone variable.

[0102] Step 7, determine whether the maximum number of iterations has been reached, if not, proceed to step 3.

[0103] Step 8. The ant path with the maximum fitness function in the iterations is selected as the final matching path of the algorithm, and the matching grid points are converted into longitude and latitude as the output matching results.

[0104] Find the ant with the largest fitness function among all iterations:

[0105] ,

[0106] Then, based on the path that the ant has taken on the geomagnetic map grid, the points on the grid can be converted into longitude and latitude.

[0107] In order to verify the effectiveness of the algorithm, this paper generates an IGRF geomagnetic map of the Chengdu area with a size of 4 km. 6km, the magnetic map interval is 30m, the initial error of the route is 80m, the number of sampling points is 30, the magnetic measurement noise is 0.5nt Gaussian noise, and the simulation process is implemented on a 12th Gen Intel(R) Core(TM) i7-12700H 2.70GHz computer using the corresponding software. The path matching results are shown in Figure 2 As shown in Figure 2, the path matching error is Figure 3 As shown, the results show that compared with the traditional geomagnetic algorithm, the present invention has obvious improvements in matching accuracy and reliability.

[0108] The specific embodiments described above further illustrate the objectives, technical solutions and beneficial effects of the present invention in detail. It should be understood that the above description is only a specific embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of the present invention should be included in the scope of protection of the present invention.

Claims

1. A geomagnetic matching navigation algorithm based on an improved ant colony algorithm, characterized in that: The following steps are involved: Step 1: Collect geomagnetic information in real time through a three-axis geomagnetic sensor module and an accelerometer module; Step 2: Calculate the magnetic heading angle of each point based on the collected geomagnetic information, and establish a discrete geomagnetic database in combination with the three-axis geomagnetic data; Step 3: Initialize the parameters of the ant colony optimization algorithm, including the pheromone volatilization factor , maximum number of iterations , the intensity factor Q of the pheromone increment, the pheromone importance factor , heuristic function importance factor , directional weight factor , the number of ants m, the geomagnetic map, and the pheromone matrix at the same latitude as the geomagnetic map; Step 4: Start the iteration, place m ants at the starting point, calculate the state transition probability according to pheromone, the heuristic function calculated based on the geomagnetic database, and the heading angle weighted function, and select the next node for each ant until the measurement sequence length is reached; Step 5: Calculate the global similarity metric function f of each ant and calculate the fitness of each ant , record the average fitness of all ants in each iteration ; Step 6: For each ant, calculate the pheromone concentration, and update the path of the pheromone with a fitness better than the average fitness value based on the pheromone concentration, and limit the upper and lower limits of the pheromone and the pheromone increment; Step 7: Determine whether the maximum number of iterations has been reached , if not reached, transfer to step 4; Step 8: The ant path corresponding to the maximum value of the fitness function is selected as the matching path of the final algorithm in the iteration of times, and the index of the ant corresponding to the maximum value of the fitness function in the pheromone matrix is ​​substituted into the geomagnetic map grid, which can be converted into longitude and latitude as the output matching result.

2. The geomagnetic matching navigation algorithm based on the improved ant colony algorithm according to claim 1 is characterized in that: The step 2 of calculating the magnetic heading angle includes: When the multi-rotor drone is in any posture, the measurement output of the accelerometer in the body coordinate system is: ， ， ， in, represents the acceleration vector, They represent the components of the acceleration on the x, y, and z axes in the body coordinate system, , denote the pitch angle and roll angle respectively; The magnetic field strength of the x, y, and z axes output by the magnetometer in the body coordinate system is , correct the tilt angle and convert the data of the body coordinate system to the horizontal body coordinate system: ， ， in, , are the magnetic field components of the x and y axes of the magnetometer in the horizontal body coordinate system; Calculate the angle between the x-axis and magnetic north : ， Therefore, the magnetic heading angle of the longitudinal axis of the body coordinate system relative to true north is for: ， In the formula, Indicates the magnetic declination between true north and magnetic north.

3. The geomagnetic matching navigation algorithm based on the improved ant colony algorithm according to claim 1 is characterized in that: In step 4, the steps of searching using m ants are as follows: At the t-th level of iteration, the probability of the k-th ant moving from point i on the grid to point j is for: ， Among them, s is the set of positions that the kth ant can reach in one step at point i, is the pheromone intensity of point j at the t-th level iteration, point j is the one-step arrival position of point i, is the heuristic function, as shown below: ， in, is the magnetic field strength of point j on the geomagnetic map, It represents the magnetic field strength of the t-th point in the magnetic measurement sequence. Here, the t-th point corresponds to the t-th level iterative solution one by one, and the corresponding position is point j on the geomagnetic map. is the heading angle weighting function, as shown below: ， in, is the magnetic heading angle of point j relative to point i on the geomagnetic map, with north-east as positive. It is the magnetic heading angle obtained by the sensor in real time.

4. The geomagnetic matching navigation algorithm based on the improved ant colony algorithm according to claim 1 is characterized in that: The global similarity measurement function of the kth ant in step 5 is As shown below: ， In the formula, is the global similarity measurement function, N is the matching length, It represents the total magnetic field intensity value on the magnetic map at step i with p as the starting point. , , Indicates the north component, east component, and longitudinal component of the corresponding position on the geomagnetic map; represents the total intensity measured by the magnetic sensor, , , They represent the north component, east component, and longitudinal component obtained by the magnetic sensor respectively; The fitness of the kth ant is: 。 5. The geomagnetic matching navigation algorithm based on the improved ant colony algorithm according to claim 1 is characterized in that: In step 6, the pheromone concentration is calculated, and the path of updating the pheromone whose fitness is better than the average fitness value includes: ， ， so: ， In the formula, is the pheromone increment of the mth ant, is the total pheromone increment of each iterative path, Q is the intensity factor of the pheromone increment, is the mean of all ant fitness functions, m is the number of ants, NC is the number of iterations, It is a pheromone volatile factor. Indicates the lower limit of pheromone concentration, Indicates the upper limit of pheromone concentration, is the pheromone variable.

6. The geomagnetic matching navigation algorithm based on improved ant colony algorithm according to claim 1, characterized in that: The step 8 comprises: Find the ant with the largest fitness function among all iterations: ， According to the above, the ant with the largest fitness function is selected, and the index of the ant in the pheromone matrix is ​​substituted into the grid of the geomagnetic map, which can be converted into longitude and latitude as the output matching result.

Images