A geomagnetic matching navigation algorithm based on improved ant colony algorithm

Abstract

The present invention discloses a geomagnetic matching navigation algorithm based on an improved ant colony algorithm, belonging to the field of geomagnetic navigation, control, and guidance. The implementation of the algorithm includes the following steps: obtaining initial position information, magnetic measurement values ​​of geomagnetic sensor detection data, and calculating the magnetic heading angle; initializing the ant colony algorithm parameters; placing ants at the initial position and transferring them to the next node according to the state transition probability formula until a magnetic measurement sequence is reached; calculating and recording the global similarity metric function of each ant and updating the pheromone; and when the maximum number of iterations is reached, outputting the optimal ant path to obtain the optimal geomagnetic navigation trajectory. The present invention utilizes the randomness of the ant colony algorithm's search and the positive feedback behavior to achieve a closer approach to the optimal solution. By using this algorithm, the optimal trajectory is obtained, thereby improving the accuracy of geomagnetic navigation matching.

Description

Technical Field

[0001] The present 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 a multidisciplinary engineering discipline. With economic development, scientific and technological advancements, and the development of transportation, the demand for navigation systems is increasing. Advances in key technologies such as computers, electronics, and signal processing have enabled the use of electronic, optical, and magnetic methods to locate and navigate moving objects by measuring parameters related to their positioning. This has led to a diversification of navigation technology, and the demand for this technology is also increasing.

[0003] Current navigation systems primarily include inertial navigation, geomagnetic matching navigation, celestial navigation, gravity matching navigation, and satellite navigation. While satellite navigation offers high positioning accuracy, its signals are weak and susceptible to interference, making reception difficult in some environments. Terrain matching navigation is advantageous for integrated navigation research in areas with large terrain variations, but its effectiveness is limited in areas with less pronounced terrain, such as deserts and oceans. While celestial navigation can provide accurate position and attitude information, it also suffers from intermittent transmission signals and susceptibility to climatic conditions. Geomagnetic navigation can, to some extent, address the shortcomings of existing navigation systems and represents a promising navigation technology.

[0004] In recent years, driven by relevant researchers, geomagnetic matching navigation technology has achieved remarkable success and is now widely used in a wide range of fields, including aircraft control, mineral exploration, earthquake monitoring and forecasting, missile guidance, anti-submarine warfare, downhole positioning, and underwater vehicle positioning. Furthermore, the investment cost of geomagnetic navigation equipment is lower than that of inertial navigation, making it of great 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 its latitude, longitude, and altitude. The process of detecting magnetic field values ​​is a passive process, with no external signals transmitted, making it highly concealed. Geomagnetic matching involves measuring the local magnetic field strength along the path of an object using a magnetometer, comparing it to a pre-stored geomagnetic map, and then matching the actual track position.

[0005] Despite its numerous advantages, geomagnetic matching navigation technology still faces several challenges in practical application. First, the measurement accuracy and stability of the geomagnetic field directly impact navigation precision, necessitating high-precision geomagnetic sensors. Second, constructing and updating geomagnetic maps is a complex process, requiring extensive field measurements and data processing. Furthermore, localized anomalies and disturbances in the geomagnetic field (such as magnetic mineral deposits and artificial magnetic fields) can also affect navigation accuracy. Traditional ant-based geomagnetic matching algorithms primarily rely on the total intensity of the geomagnetic field for matching, failing to fully utilize the vector information of the geomagnetic field, resulting in limited matching accuracy. Furthermore, the same pheromone concentration in the initial iteration leads to uncertain path selection, while relying solely on the total intensity of the geomagnetic field for matching results in significant uncertainty in movement trajectories. The paths of previous ants can interfere with subsequent solutions, potentially leading to the dominance of suboptimal solutions during the search process. Furthermore, the pheromones in traditional algorithms are distributed along the paths between grid nodes, making them inefficient for storage and computation. Summary of the Invention

[0006] In response to 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 angles, and pheromone update of the three components of the geomagnetic field, the geomagnetic matching algorithm is optimized, effectively improving navigation accuracy and efficiency, especially showing significant advantages 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) 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 volatility 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 the pheromone and the data from 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.

[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 fitness function of all ants .

[0014] (6) Calculate the pheromone concentration, update the pheromone path with fitness better than the mean, and set upper and lower limits for pheromone and pheromone increment.

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

[0016] (8) In The ant path with the maximum fitness function in the iterations is selected as the final matching path of the algorithm. 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 Earth's magnetic 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 level, the three-axis data measured are the geomagnetic components of the machine system. For the two horizontal components, their vector sum always points toward magnetic north.

[0019] When the multirotor 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 along 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 body coordinate system data 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 true north and magnetic north. , so the heading angle of the vertical axis of the body coordinate 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 to point j on the grid 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 global similarity metric function is calculated using the geomagnetic vector. The global similarity metric function of the kth ant is as follows:

[0041] ,

[0042] Where, 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 represent the north component, east component, and longitudinal component obtained by the magnetic sensor respectively.

[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] Where, 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 the 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 geomagnetic map grid, which can be converted into longitude and latitude as the output matching result.

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

[0056] This invention effectively addresses the problems of traditional ant colony geomagnetic matching algorithms, such as uncertain path selection caused by the same pheromone concentration during the initial iteration, and low navigation accuracy caused by relying solely on the total strength of the geomagnetic field for matching, which can lead to local optima. When the ants select the next node, a heuristic function and a weighted magnetic heading function are introduced. The local heuristic function uses a single-point MAD algorithm to calculate the difference between the magnetic measurement value and the magnetic field value at the corresponding point on the magnetic map, using its reciprocal as the local heuristic function. The magnetic heading angle is calculated using a three-axis magnetometer, thereby optimizing the ants' selection of the optimal path at a specific local point.

[0057] The size of pheromone is updated by 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 according to the present invention;

[0060] Figure 2 This is 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 the geomagnetic matching navigation algorithm based on the improved ant colony algorithm of the present invention. DETAILED DESCRIPTION

[0062] The present invention is further described in detail below with reference to 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 multirotor 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 body coordinate system data 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 vertical axis of the body coordinate relative to true north is:

[0080] ,

[0081] Step 3: Initialize the parameters of the ant colony optimization algorithm, including the pheromone volatility 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 the iteration, place m ants at the starting point, calculate the heuristic function based on the pheromone and the data from 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 probability of transferring from point i to point j on the grid is calculated 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 global similarity metric function is calculated using the geomagnetic vector. The global similarity metric function of the kth ant is as follows:

[0092] ,

[0093] Where, is the global similarity measurement 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 represent the north component, east component, and longitudinal component obtained by the magnetic sensor respectively.

[0094] The fitness of the kth ant is:

[0095] ,

[0096] Step 6: Calculate the pheromone concentration, update the pheromone path with a fitness better than the mean, and set upper and lower limits for pheromone and pheromone increment.

[0097] ,

[0098] ,

[0099] so:

[0100] ,

[0101] Where, 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 the 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 using the corresponding software on a 12th Gen Intel(R) Core(TM) i7-12700H 2.70GHz computer. The path matching results are shown in Figure 2 As shown, the path matching error is Figure 3 As shown in the figure, the results show that compared with the traditional geomagnetic algorithm, the present invention has significantly improved the 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 are only specific embodiments of the present invention and are 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 the three-axis geomagnetic sensor module and the accelerometer module; Step 2: Calculate the magnetic heading angle of each point based on the collected geomagnetic information, and build 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 volatility factor , maximum number of iterations , the intensity factor Q of the pheromone increment, the pheromone importance factor α, the 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 based on pheromones, 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 function of all ants in each iteration ; Step 6: Calculate the pheromone concentration for each ant, and update the pheromone path with a fitness better than the average fitness value based on the pheromone concentration, and set upper and lower limits for the pheromone and pheromone increment; Step 7: Determine whether the maximum number of iterations has been reached If not reached, go 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. 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: Calculating the magnetic heading angle in step 2 includes: When the multirotor drone is in any posture, the measurement output of the accelerometer in the body coordinate system is: ， ， ， in, represents the acceleration vector, Respectively represent the components of 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: ， Where, 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 k-th ant can reach in one step at point i, is the pheromone intensity of point j when solving the problem 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 at 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, 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 in the following formula: ， Where, is the global similarity measurement function, N is the matching length, It represents the total magnetic field intensity value on the magnetic map of 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 4 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: ， Where, 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 average fitness function of all ants, m is the number of ants, NC is the number of iterations, It is the 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 the improved ant colony algorithm according to claim 4 is 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 geomagnetic map grid, which can be converted into longitude and latitude as the output matching result.

Images