Unmanned vehicle geomagnetic / odometer fusion real-time positioning method and system based on adaptive sequence particle filtering

Abstract

The invention discloses an unmanned vehicle geomagnetic / odometer fusion real-time positioning system based on adaptive sequence particle filtering, and belongs to an unmanned vehicle positioning technology. The method is provided for solving the problems of poor anti-noise capability, low calculation efficiency and the like of geomagnetic navigation. According to the technical scheme, the three components are added into the geomagnetic navigation method, the number of features used in the navigation process is increased, and positioning errors are reduced. In the navigation process, the weight of each feature is calculated in real time based on the stability of each component, and the contribution degree of each feature during navigation is adjusted. Geomagnetic positioning is carried out based on adaptive KLD sequence particle filtering, and the number of particles is dynamically limited in the positioning process, so that high-efficiency geomagnetic positioning is realized. Real-time weight calculation is carried out based on the stability of each component, and the contribution degree of each component in the subsequent positioning process is dynamically adjusted; and components which are seriously interfered are screened out through global weights, so that the calculation amount of a subsequent positioning process is effectively reduced while the positioning precision is not influenced, and the calculation efficiency is improved.

Description

Technical Field

[0001] The present invention relates to a geomagnetic / odometer fusion real-time positioning system for an unmanned vehicle, belonging to the field of unmanned vehicle positioning technology. Background Art

[0002] Autonomous navigation of unmanned ground vehicles (UGVs) is a core capability for carrying out missions such as military reconnaissance, disaster relief, and industrial automation. Accurate and reliable navigation systems enable UGVs to operate safely in complex and dynamic environments, avoid obstacles, and efficiently reach their target locations. Among the numerous sensors and technologies used for positioning and path planning, odometry is a key component. By measuring wheel rotation or other proprioceptive data, it provides continuous motion estimation, supporting the real-time requirements of short-term navigation. However, because errors accumulate over time, odometry is often integrated with technologies such as inertial navigation to improve the robustness of long-term navigation.

[0003] Geomagnetic navigation, a positioning technology that utilizes the Earth's magnetic field, offers global coverage, all-weather capability, and low cost, making it widely used in unmanned vehicles, aerospace, ocean exploration, and other fields. Geomagnetic navigation determines precise position or attitude by measuring the strength and direction of the Earth's magnetic field at the current location and matching it with a pre-built geomagnetic map. Because the Earth's magnetic field is a global, natural field whose strength and direction are fixed and unique at a specific geographic location, geomagnetic navigation can obtain absolute position information. Combined with the odometry system, it can correct for accumulated errors and achieve high-precision navigation for UGVs. However, geomagnetic navigation suffers from significant noise interference and low computational efficiency, hindering its practicality.

[0004] Document number CN119169081B discloses a geomagnetic adaptation area positioning method and device for geomagnetic navigation, which specifically includes: determining a geomagnetic feature map corresponding to the target area based on the geomagnetic information map corresponding to the target area. The geomagnetic feature map is input into a scale division model, and the scale division model generates a geomagnetic feature map corresponding to the geomagnetic information map at each image resolution. For each scale feature map, a candidate geomagnetic adaptation area in the target area at the corresponding image resolution is determined. Based on each candidate geomagnetic adaptation area, the geomagnetic adaptation area of the target area is determined. Compared with the current technology of manually determining the geomagnetic adaptation area, the method of this specification is more intelligent and more automated. While improving the efficiency and accuracy of determining the geomagnetic adaptation area, it also greatly saves human resources and time costs, thereby improving the execution efficiency of subsequent geomagnetic route planning based on the geomagnetic adaptation area. However, this prior art does not mention geomagnetic / odometer fusion positioning, nor does it mention how to improve the noise resistance of geomagnetic navigation.

[0005] Therefore, it is necessary to propose an innovative geomagnetic / odometer fusion positioning method for unmanned vehicles to improve its anti-noise ability and computational efficiency. Summary of the Invention

[0006] The technical problems to be solved by the present invention are:

[0007] In order to solve the problems of poor anti-noise ability and low computational efficiency of geomagnetic navigation, the present invention provides a real-time positioning method and system for unmanned vehicle geomagnetic / odometer fusion based on adaptive sequential particle filtering.

[0008] The technical solution adopted by the present invention to solve the above technical problems is:

[0009] A real-time positioning method for an unmanned vehicle based on geomagnetic / odometer fusion based on an adaptive sequential particle filter, the method comprising:

[0010] (1) The horizontal component M in the geomagnetic field model H , magnetic inclination M I and magnetic declination M D Three components (the last three components of Formula 9, which are actually Formulas 6, 7, and 8) are added to the geomagnetic navigation method to increase the number of features used in the navigation process and reduce positioning errors;

[0011] (2) The weight of each feature is calculated in real time to adjust its contribution to navigation. First, a data buffer for the measurement sequence is established. When a new data point is collected during the operation of the carrier, the starting point is removed from the sequence and the new data point is added to the end of the sequence. The global weight of the data distribution is measured by MAD to adjust the overall weight of each component.

[0012] (3) Geomagnetic positioning is performed based on the adaptive KLD sequence particle filter, and the number of particles is dynamically limited during the positioning process to achieve high-efficiency geomagnetic positioning; combined with the motion trajectory inference model of the unmanned vehicle odometer, geomagnetic positioning is performed based on the particle filter, and KLD resampling is used to improve the computational efficiency of the algorithm.

[0013] The specific implementation process of the method is:

[0014] (1) The horizontal component M in the geomagnetic field model H , magnetic inclination M I and magnetic declination M D Three components are added to the geomagnetic navigation method to increase the number of features used in the navigation process and reduce positioning errors;

[0015] Assume that the UGV used for positioning is equipped with a three-axis fluxgate magnetometer and an inertial measurement unit (IMU), which are used to obtain geomagnetic field data and vehicle attitude angle data respectively, and the magnetometer coordinate axes, IMU coordinate axes and UGV carrier coordinate axes are all aligned;

[0016] During the positioning process, the data collected by the magnetometer in the UGV carrier coordinate system is converted into a coordinate system; assuming that the three-axis attitude angles measured by the IMU are θ, γ, and φ, and the vector geomagnetic data measured by the magnetometer is M X '、M Y '、M Z ', the north component of the geomagnetic field vector M X , eastward component M Y and the vertical component M Z The calculation is as follows:

[0017]

[0018]

[0019] The total geomagnetic field is calculated as follows:

[0020]

[0021] Through the above components, calculate the horizontal component M H , magnetic inclination M I and magnetic declination M D :

[0022]

[0023] Therefore, during the positioning process, at the kth moment, the geomagnetic data used are as follows:

[0024]

[0025] (2) Calculate the weight of each feature in real time and adjust its contribution to navigation;

[0026] First, create the data buffer for the measurement sequence:

[0027]

[0028] where q m is a data point in the buffer Q, N D is the sequence length;

[0029] When a new data point is collected during the operation of the carrier, the starting point is moved out of the sequence Q:

[0030] Q→Q-q1 (11)

[0031] Add new data points to the end of the series:

[0032]

[0033] First calculate the median absolute deviation (MAD) of the data:

[0034] mad(X m )=median(|X m -median(X m )|) (13)

[0035] The Huber weight is used to measure the stability of the data distribution of each component and dynamically adjust the weight:

[0036]

[0037] Among them, X m is a component of geomagnetic data (m=1,2,··,7),∈ m is the error of geomagnetic data, median(x) represents the median calculation, δ c is the threshold of Huber weight, w h,m Represents the Huber weight of a certain component of geomagnetic data;

[0038] Combined with MAD to measure the global weight of data distribution, the overall weight of each component is further adjusted:

[0039]

[0040] Where mad(x) represents MAD calculation, w g,m Represents the global weight of a certain component of geomagnetic data;

[0041] Finally, the weights of each component used in the positioning process are calculated as follows:

[0042] w f,m =w h,m w g,m (17)

[0043] When the weight of a component is lower than the threshold w threshold , that is, satisfying w f,m <w threshold When , it is considered that this component is seriously affected by noise. In subsequent calculations, this component is excluded to reduce the amount of calculation in the positioning process and improve positioning efficiency:

[0044]

[0045] (3) Geomagnetic positioning is performed based on adaptive KLD sequence particle filtering, and the number of particles is dynamically limited during the positioning process to achieve high-efficiency geomagnetic positioning;

[0046] The motion trajectory inference model of the unmanned vehicle odometer is as follows:

[0047]

[0048] Where v is the current velocity of the robot, w is the current angular velocity of the robot, and dt is the sampling interval. d is the distance from the driving wheel to the center of the two wheels, dθ is the current angle increment, ds is the current displacement of the robot, and r L 、r R is the radius of the left and right wheels ω L 、ω R is the angular velocity of the left and right wheels;

[0049] Combined with the odometer model, geomagnetic positioning is performed based on particle filtering: first, the particles are initialized, and the filter adjusts the three parameters. The number of particles is N, and the i-th particle p i As shown in formula (2):

[0050]

[0051] in d i Used to adjust the odometer r L 、r R , d parameters.

[0052] In the sequence buffer Q, through p i Generate a corresponding trajectory and match it with the pre-established geomagnetic reference map. i The corresponding weight w i As follows:

[0053]

[0054] In the formula is the value of a component of the magnetic field at the jth position of the trajectory on the geomagnetic reference map, is the jth measured value of a component of the magnetic field in the buffer zone Q. f,m for The corresponding weight, when w f,m <w threshold When , this component does not participate in the calculation; λ is a constant that adjusts the overall particle weight.

[0055] After all particle weights are calculated, the weights need to be normalized:

[0056]

[0057] Final result p rst The calculation is as follows:

[0058]

[0059] KLD resampling is used to improve the computational efficiency of the algorithm and achieve optimal performance. The minimum number of particles is calculated by the following formula:

[0060]

[0061] where n s represents the number of subspaces, z 1-ρ is the upper bound of the 1-ρ quantile of the standard normal distribution.

[0062] Before performing particle filtering, a set of data is needed to calculate the parameters r of the odometer. L 、r R , d are pre-calibrated to avoid large errors between the odometer trajectory and the actual trajectory; the pre-calibrated parameters are recorded as r L0 、r R0 , d0; During the execution of the algorithm, each particle needs to be d i Limit the range to reduce the possibility of particle filter divergence; The range is The range is d i The range is (d0-k d , d0+k d ).

[0063] X m is a component of the geomagnetic data (m=1, 2, ··, 7). When m is 1, 2, 3, 4, 5, 6 and 7, the corresponding meanings are the 1st, 2nd, 3rd, 4th, 5th, 6th and 7th elements of formula (9), respectively.

[0064] The algorithm flow steps for geomagnetic positioning based on particle filtering are as follows:

[0065] Establishing the data buffer of the measurement sequence;

[0066] Get the current magnetometer vector measurement value, attitude angle, and odometer data, calculate the seven-dimensional geomagnetic data components, and add them to the buffer to form a data sequence;

[0067] Dynamic weight allocation and feature screening based on the stability of each component;

[0068] Generate particles and calculate particle weights based on odometry data;

[0069] Calculate the sequence particle filter positioning results;

[0070] Perform adaptive KLD resampling on particles.

[0071] If the navigation reaches the end point, it ends. Otherwise, the current magnetometer vector measurement value, attitude angle, and odometer data are re-acquired, and the seven-dimensional geomagnetic data components are calculated and added to the buffer to form a data sequence until the navigation reaches the end point.

[0072] A real-time positioning system for an unmanned vehicle using geomagnetic field / odometer fusion based on an adaptive sequential particle filter. The system comprises a program module corresponding to the steps of the above-mentioned technical solution, and executes the steps of the real-time positioning method for an unmanned vehicle using geomagnetic field / odometer fusion based on an adaptive sequential particle filter when running.

[0073] A computer-readable storage medium stores a computer program configured to implement the steps of a real-time positioning method for an unmanned vehicle based on geomagnetic / odometer fusion based on adaptive sequential particle filtering when called by a processor.

[0074] The present invention has the following beneficial technical effects:

[0075] 1. This invention effectively solves the problem of geomagnetic navigation being interfered with by noise. It uses the seven-dimensional components of the geomagnetic field for UGV geomagnetic navigation, reduces the problem of inaccurate positioning caused by the influence of geomagnetic measurement noise, and effectively improves the reliability and practicality of the algorithm.

[0076] 2. This invention effectively addresses the computational complexity of geomagnetic navigation algorithms. It performs real-time weighting based on the stability of each component, dynamically adjusting the contribution of each component in the subsequent positioning process. It also uses global weighting to filter out components that are subject to significant interference, effectively reducing the computational complexity of the subsequent positioning process without compromising positioning accuracy. This invention proposes an innovative geomagnetic / odometer fusion positioning method for unmanned vehicles, which also improves computational efficiency.

[0077] 3. The present invention further reduces the computational complexity of the geomagnetic navigation algorithm, performs geomagnetic positioning based on an improved adaptive KL divergence (Kullback–Leibler Divergence, KLD) sequence particle filter, and dynamically limits the number of particles during the positioning process to achieve high-efficiency geomagnetic positioning. BRIEF DESCRIPTION OF THE DRAWINGS

[0078] Figure 1 It is a flowchart of the algorithm of the present invention. Figure 2 is the geomagnetic field component map, Figure 3 is the algorithm flow chart, Figure 4 This is an algorithm comparison diagram (comparing the positioning trajectory of the traditional particle filter algorithm and the algorithm proposed in this invention); Figure 5A partial experimental path diagram. DETAILED DESCRIPTION

[0079] Give Attachment Figure 1-5 The implementation of the unmanned vehicle geomagnetic / odometer fusion real-time positioning method based on adaptive sequential particle filtering of the present invention is described as follows:

[0080] The flowchart of the proposed algorithm is shown in Figure 1 shown.

[0081] (1) In existing geomagnetic navigation methods, the total geomagnetic field M is often used. F , the north component M of the geomagnetic field vector X , eastward component M Y and the vertical component M Z However, during geomagnetic navigation, since the magnetic sensor is susceptible to noise interference, the geomagnetic data of some components may differ greatly from the geomagnetic reference map, resulting in problems such as mismatching and increased positioning error. Using more features will reduce the probability of mismatching and improve positioning accuracy. In the geomagnetic field model, commonly used components also include the horizontal component M H , magnetic inclination M I and magnetic declination M D ,like Figure 2 The present invention adds the above three components to the geomagnetic navigation method, increases the number of features used in the navigation process, and reduces positioning errors.

[0082] Assume that the UGV used for positioning is equipped with a three-axis fluxgate magnetometer and an inertial measurement unit (IMU), which are used to obtain geomagnetic field data and vehicle attitude angle data respectively, and the magnetometer coordinate axis, IMU coordinate axis and UGV carrier coordinate axis are all aligned. During the positioning process, the data collected by the magnetometer is the data in the UGV carrier coordinate system, and the coordinate system conversion is required. Assume that the three-axis attitude angles measured by the IMU are θ, γ, and φ, and the vector geomagnetic data measured by the magnetometer is M X '、M Y '、M Z ', the north component of the geomagnetic field vector M X , eastward component M Y and the vertical component M Z The calculation is as follows:

[0083]

[0084] The total geomagnetic field is calculated as follows:

[0085]

[0086] Through the above components, calculate the horizontal component M H, magnetic inclination M I and magnetic declination M D :

[0087]

[0088] Therefore, during the positioning process, at the kth moment, the geomagnetic data used by the present invention is as follows:

[0089]

[0090] (2) In a dynamic environment, noise is complex. Therefore, the noise interference and stability of different components may change dynamically. Therefore, during the navigation process, the present invention calculates the weight of each feature in real time based on the stability of each component and adjusts its contribution to navigation.

[0091] First, create the data buffer for the measurement sequence:

[0092]

[0093] where q m is a data point in the buffer Q, N D is the sequence length.

[0094] When a new data point is collected during the operation of the carrier, the starting point is moved out of the sequence Q:

[0095] Q→Q-q1 (11)

[0096] Add new data points to the end of the series:

[0097]

[0098] Huber weight is a robust statistical method that can reduce the impact of outliers in geomagnetic measurements on fusion results.

[0099] First calculate the median absolute deviation (MAD) of the data:

[0100] mad(X m )=median(|X m -median(X m )|) (13)

[0101] The Huber weight is used to measure the stability of the data distribution of each component and dynamically adjust the weight:

[0102]

[0103] Among them, X mis a component of geomagnetic data (m=1,2,··,7),∈ m is the error of geomagnetic data, median(x) represents the median calculation, δ c is the threshold of Huber weight, w h,m Represents the Huber weight of a component of geomagnetic data.

[0104] The Median Absolute Deviation (MAD) is a robust statistic used to measure the degree of data dispersion. It uses the median rather than the mean as a benchmark and is not affected by extreme values or outliers in geomagnetic measurements. Combined with the MAD to measure the global weight of the data distribution, the overall weight of each component is further adjusted:

[0105]

[0106] Where mad(x) represents MAD calculation, w g,m Represents the global weight of a component of geomagnetic data.

[0107] Finally, the weights of each component used in the positioning process are calculated as follows:

[0108] w f,m =w h,m w g,m (17)

[0109] When the weight of a component is lower than the threshold w threshold , that is, satisfying w f,m <w threshold When , it is considered that this component is seriously affected by noise. In subsequent calculations, this component is excluded to reduce the amount of calculation in the positioning process and improve positioning efficiency:

[0110]

[0111] (3) In order to further reduce the computational complexity of the geomagnetic navigation algorithm, a geomagnetic positioning algorithm based on adaptive KLD sequence particle filtering is proposed. The number of particles is dynamically limited during the positioning process to achieve high-efficiency geomagnetic positioning.

[0112] The motion trajectory inference model of the unmanned vehicle odometer is as follows:

[0113]

[0114] Where v is the current velocity of the robot, w is the current angular velocity of the robot, and dt is the sampling interval. d is the distance from the driving wheel to the center of the two wheels, dθ is the current angle increment, ds is the current displacement of the robot, and r L 、r R is the radius of the left and right wheels ωL 、ω R is the angular velocity of the left and right wheels.

[0115] Combined with the odometer model, geomagnetic positioning is performed based on particle filtering. First, the particles are initialized and the filter adjusts the three parameters. The number of particles is N, and the i-th particle p i As shown in formula (2):

[0116]

[0117] in d i Used to adjust the odometer r L 、r R , d parameters.

[0118] In the sequence buffer Q, through p i Generate a corresponding trajectory and match it with the pre-established geomagnetic reference map. i The corresponding weight w i As follows:

[0119]

[0120] In the formula is the value of a component of the magnetic field at the jth position of the trajectory on the geomagnetic reference map, is the jth measured value of a component of the magnetic field in the buffer zone Q. f,m for The corresponding weight, when w f,m <w threshold When , this component does not participate in the calculation. λ is a constant that adjusts the overall particle weight.

[0121] After all particle weights are calculated, the weights need to be normalized:

[0122]

[0123] Final result p rst The calculation is as follows:

[0124]

[0125] KLD resampling is an adaptive particle filter resampling method based on information theory. Its core idea is to more efficiently approximate the target distribution by dynamically adjusting the number of particles while reducing computational overhead. This paper proposes to use KLD resampling to improve the algorithm's computational efficiency and achieve optimal performance. The minimum number of particles is calculated using the following formula:

[0126]

[0127] where n s represents the number of subspaces, z 1-ρ is the upper bound of the 1-ρ quantile of the standard normal distribution.

[0128] Before performing particle filtering, a set of data is needed to calculate the parameters r of the odometer. L 、r R , d are pre-calibrated to avoid large errors between the odometer trajectory and the actual trajectory. The pre-calibrated parameters are recorded as r L0 、r R0 , d0. During the execution of the algorithm, each particle needs to be d i The range of the particle filter is limited to reduce the possibility of particle filter divergence. The range is The range is d i The range is (d0-k d , d0+k d ).

[0129] The process of the algorithm of the present invention is as follows Figure 3 , the steps can be summarized as follows:

[0130] Step 1: Create a data buffer for the measurement sequence.

[0131] Step 2: Obtain the current magnetometer vector measurement value, attitude angle, and odometer data. Calculate the seven-dimensional geomagnetic data components and add them to the buffer to form a data sequence.

[0132] Step 3: Dynamic weight allocation and feature screening based on the stability of each component.

[0133] Step 4: Generate particles and calculate particle weights based on odometry data.

[0134] Step 5: Calculate the sequence particle filter positioning results.

[0135] Step 6: Perform adaptive KLD resampling on particles.

[0136] Step 7: If the navigation reaches the destination, the process ends. Otherwise, go to step 2.

[0137] Example 1 (Simulation):

[0138] Experimental environment and configuration:

[0139] Experimental scenario: This paper applies the positioning system to a simulated UGV motion environment equipped with an odometer and geomagnetic sensors, comparing the results with real-world trajectories. The experiment uses a 100m x 100m simulated map, controlling the UGV to move along multiple curved, random trajectories.

[0140] Experimental environment: CPU: Intel(R) Core i7-12800HX@2.00GHz, 16G RAM, Windows 10.

[0141] In this embodiment, N D Take 10, λ take 100, w threshold is 0.15, and ρ is 0.05. c Take 1.345. The actual r of the odometer L 、r R , d are taken as 120mm, 120mm and 500mm respectively, and the pre-calibrated parameter r L0 、r R0 , d0 are 119.6mm, 120mm and 480mm respectively. Range limitation k d They are 3mm, 3mm, and 50mm respectively.

[0142] Comparison and evaluation of algorithm performance in simulation environment:

[0143] The actual trajectory, odometer trajectory, traditional contour matching algorithm, traditional particle filter algorithm and the positioning trajectory of the algorithm proposed in this invention are compared, and some experimental paths are obtained as follows: Figure 4 The average metrics of all experiments are shown in Table 1.

[0144] Table 1 Comparison of algorithm results

[0145]

[0146] Depend on Figure 1 As can be seen from Table 1, after the odometer trajectory has worked over a long distance, the error has diverged, the positioning accuracy has gradually decreased, and it cannot work alone. The positioning algorithm using traditional contour matching and traditional particle filtering works well in the early part of the trajectory. As the odometer error increases, its positioning stability gradually decreases, resulting in the introduction of more maximum errors and poor average positioning performance. The improved particle filtering algorithm of the present invention is superior to the traditional algorithm in terms of average error and maximum error. At the same time, the positioning calculation amount of the present invention is small, and the efficiency is higher than the contour matching algorithm which is also a sequence matching, but slightly lower than the traditional particle filtering algorithm of single point matching.

[0147] Example 2 (actual):

[0148] Experimental environment and configuration:

[0149] Experimental scenario: This paper applied the positioning system to a real-world UGV equipped with an odometer and geomagnetic sensor. Real-world trajectories were collected using high-precision differential GPS for comparison. The experiment used a real-world area measuring 60m x 30m, and the UGV was controlled to move in multiple curved, random trajectories.

[0150] Experimental environment: CPU: Intel(R) Core i7-12800HX@2.00GHz, 16G RAM, Windows 10.

[0151] In this embodiment, N D Take 15, λ take 100, w threshold is 0.15, and ρ is 0.05. c Take 1.345. The odometer pre-calibrated parameters are given by the manufacturer, r L0 、r R0 , d0 are 165mm, 165mm and 583mm respectively. Range limitation k d They are 3mm, 3mm, and 50mm respectively.

[0152] Comparison and evaluation of algorithm performance in actual environment:

[0153] The real trajectory, odometer trajectory, contour matching algorithm based on sequence matching, particle filter algorithm based on single point matching and positioning trajectory of the algorithm proposed in this invention are compared, and some experimental paths are obtained as follows: Figure 5 The average metrics of all experiments are shown in Table 2.

[0154] Table 2 Comparison of algorithm results

[0155]

[0156] Depend on Figure 5 As shown in Table 2, similar to the simulation, the odometry trajectory exhibits significantly increased error after long-distance travel, rendering it incapable of operating independently. Positioning algorithms using traditional contour matching and particle filtering still suffer from poor positioning stability in the latter half of the process. In real-world environments, the improved particle filtering algorithm of this invention outperforms traditional algorithms in terms of average and maximum error. Furthermore, positioning efficiency is higher than that of the contour matching algorithm, but slightly lower than that of the traditional particle filtering algorithm using single-point matching.

[0157] It has been verified that the method proposed in the present invention solves the technical problem proposed in the present invention. The method of the present invention has been verified through simulation experiments and practical applications to achieve the technical effects claimed by the present invention.

[0158] The algorithm (method) proposed in the present invention is the underlying technical core of the present invention, and various products can be derived based on the algorithm.

[0159] Based on the algorithm (method) proposed in the present invention, a real-time positioning system for geomagnetic field / odometer fusion of unmanned vehicles based on adaptive sequential particle filtering is developed using a programming language. The system has program modules corresponding to the steps of the above-mentioned technical solution, and executes the steps of the above-mentioned real-time positioning method for geomagnetic field / odometer fusion of unmanned vehicles based on adaptive sequential particle filtering during operation.

[0160] The developed system (software) computer program is stored on a computer-readable storage medium. When called by a processor, the computer program is configured to implement the steps of the aforementioned method for real-time positioning of an unmanned vehicle using geomagnetic field / odometer fusion based on adaptive sequential particle filtering. This materializes the present invention on a carrier, becoming a computer program product.

[0161] Various implementations of the systems and techniques described herein can be realized in digital electronic circuit systems, integrated circuit systems, dedicated ASICs (application specific integrated circuits), computer hardware, firmware, software, and / or combinations thereof. These various implementations can include being implemented in one or more computer programs that can be executed and / or interpreted on a programmable system that includes at least one programmable processor, which can be a special purpose or general purpose programmable processor that can receive data and instructions from a storage system, at least one input device, and at least one output device, and transmit data and instructions to the storage system, the at least one input device, and the at least one output device.

[0162] The computer programs (also referred to as programs, software, software applications, or code) of the present invention include machine instructions for a programmable processor and can be implemented using high-level procedural and / or object-oriented programming languages, and / or assembly / machine languages. As used herein, the terms "machine-readable medium" and "computer-readable medium" refer to any computer program product, device, and / or means (e.g., a magnetic disk, optical disk, memory, programmable logic device (PLD)) for providing machine instructions and / or data to a programmable processor, including machine-readable media that receive machine instructions as machine-readable signals. The term "machine-readable signal" refers to any signal for providing machine instructions and / or data to a programmable processor.

[0163] In summary, the present invention provides the following conclusions:

[0164] This invention utilizes geomagnetic feature expansion, dynamic weight calculation and feature screening, and an adaptive KLD particle filter method to achieve UGV geomagnetic / odometer fusion positioning. Experiments demonstrate that the proposed algorithm outperforms traditional geomagnetic positioning algorithms in terms of positioning accuracy, effectively reducing odometry cumulative error over long distances. Furthermore, it demonstrates relatively good efficiency in sequence matching algorithms. The positioning system designed in this invention achieves high-precision and efficient UGV geomagnetic / odometer fusion positioning.

[0165] The above-mentioned implementation examples are merely for the purpose of illustrating the technical concept and workflow of the present invention in detail. Their purpose is to enable those skilled in the art to understand the contents of the present invention and implement them accordingly. They are not intended to limit the scope of protection of the present invention. Those skilled in the art will appreciate that other variations or modifications can be made based on the above description. It is not possible to enumerate all implementation methods here. Any equivalent variations or modifications made based on the technical solution of the present invention are intended to fall within the scope of protection of the present invention.

Claims

1. A real-time positioning method for unmanned vehicle geomagnetic / odometer fusion based on adaptive sequential particle filtering, wherein the method comprises: (1) The horizontal component M in the geomagnetic field model H , magnetic inclination M I and magnetic declination M D Three components are added to the geomagnetic navigation method to increase the number of features used in the navigation process and reduce positioning errors; (2) The weight of each feature is calculated in real time to adjust its contribution to navigation. First, a data buffer for the measurement sequence is established. When a new data point is collected during the operation of the carrier, the starting point is removed from the sequence and the new data point is added to the end of the sequence. The global weight of the data distribution is measured by MAD to adjust the overall weight of each component. (3) Geomagnetic positioning is performed based on the adaptive KLD sequence particle filter, and the number of particles is dynamically limited during the positioning process to achieve high-efficiency geomagnetic positioning; combined with the motion trajectory inference model of the unmanned vehicle odometer, geomagnetic positioning is performed based on the particle filter, and KLD resampling is used to improve the computational efficiency of the algorithm.

2. A real-time positioning method for unmanned vehicles based on geomagnetic / odometer fusion based on adaptive sequential particle filtering, characterized in that: The specific implementation process of the method is: (1) The horizontal component M in the geomagnetic field model H , magnetic inclination M I and magnetic declination M D Three components are added to the geomagnetic navigation method to increase the number of features used in the navigation process and reduce positioning errors; Assume that the UGV used for positioning is equipped with a three-axis fluxgate magnetometer and an inertial measurement unit (IMU), which are used to obtain geomagnetic field data and vehicle attitude angle data respectively, and the magnetometer coordinate axes, IMU coordinate axes and UGV carrier coordinate axes are all aligned; During the positioning process, the data collected by the magnetometer in the UGV carrier coordinate system is converted into a coordinate system; assuming that the three-axis attitude angles measured by the IMU are θ, γ, and φ, and the vector geomagnetic data measured by the magnetometer is M X '、M Y '、M Z ', the north component of the geomagnetic field vector M X , eastward component M Y and the vertical component M Z The calculation is as follows: The total geomagnetic field is calculated as follows: Through the above components, calculate the horizontal component M H , magnetic inclination M I and magnetic declination M D : Therefore, during the positioning process, at the kth moment, the geomagnetic data used are as follows: (2) Calculate the weight of each feature in real time and adjust its contribution to navigation; First, create the data buffer for the measurement sequence: where q m is a data point in the buffer Q, N D is the sequence length; When a new data point is collected during the operation of the carrier, the starting point is moved out of the sequence Q: Q→Q-q1 (11) Add new data points to the end of the series: First calculate the median absolute deviation (MAD) of the data: mad(X m )=median(|X m -median(X m )|) (13) The Huber weight is used to measure the stability of the data distribution of each component and dynamically adjust the weight: Among them, X m is a component of geomagnetic data (m=1,2,··,7),∈ m is the error of geomagnetic data, median(x) represents the median calculation, δ c is the threshold of Huber weight, w h,m Represents the Huber weight of a certain component of geomagnetic data; Combined with MAD to measure the global weight of data distribution, the overall weight of each component is further adjusted: Where mad(x) represents MAD calculation, w g,m Represents the global weight of a certain component of geomagnetic data; Finally, the weights of each component used in the positioning process are calculated as follows: In f,m =in h,m ·In g,m (17) When the weight of a component is lower than the threshold w threshold , that is, satisfying w f,m <w threshold When , it is considered that this component is seriously affected by noise. In subsequent calculations, this component is excluded to reduce the amount of calculation in the positioning process and improve positioning efficiency: (3) Geomagnetic positioning is performed based on adaptive KLD sequence particle filtering, and the number of particles is dynamically limited during the positioning process to achieve high-efficiency geomagnetic positioning; The motion trajectory inference model of the unmanned vehicle odometer is as follows: Where v is the current velocity of the robot, w is the current angular velocity of the robot, and dt is the sampling interval. d is the distance from the driving wheel to the center of the two wheels, dθ is the current angle increment, ds is the current displacement of the robot, and r L 、r R is the radius of the left and right wheels ω L 、ω R is the angular velocity of the left and right wheels; Combined with the odometer model, geomagnetic positioning is performed based on particle filtering: first, the particles are initialized, and the filter adjusts the three parameters. The number of particles is N, and the i-th particle p i As shown in formula (2): in d i Used to adjust the odometer r L 、r R , d parameters. In the sequence buffer Q, through p i Generate a corresponding trajectory and match it with the pre-established geomagnetic reference map. i The corresponding weight w i As follows: In the formula is the value of a component of the magnetic field at the jth position of the trajectory on the geomagnetic reference map, is the jth measured value of a component of the magnetic field in the buffer zone Q. f,m for The corresponding weight, when w f,m <w threshold When , this component does not participate in the calculation; λ is a constant that adjusts the overall particle weight. After all particle weights are calculated, the weights need to be normalized: Final result p rst The calculation is as follows: KLD resampling is used to improve the computational efficiency of the algorithm and achieve optimal performance. The minimum number of particles is calculated by the following formula: where n s represents the number of subspaces, z 1-ρ is the upper bound of the 1-ρ quantile of the standard normal distribution.

3. A real-time positioning method for unmanned vehicle geomagnetic / odometer fusion based on adaptive sequential particle filtering according to claim 2, characterized in that: Before performing particle filtering, a set of data is needed to calculate the parameters r of the odometer. L 、r R , d are pre-calibrated to avoid large errors between the odometer trajectory and the actual trajectory; the pre-calibrated parameters are recorded as r L0 、r R0 , d0; During the execution of the algorithm, each particle needs to be d i Limit the range to reduce the possibility of particle filter divergence; The range is The range is d i The range is (d0-k d , d0+k d ).

4. A real-time positioning method for unmanned vehicle geomagnetic / odometer fusion based on adaptive sequential particle filtering according to claim 3, characterized in that: X m is a component of the geomagnetic data (m=1, 2, ··, 7). When m is 1, 2, 3, 4, 5, 6 and 7, the corresponding meanings are the 1st, 2nd, 3rd, 4th, 5th, 6th and 7th elements of formula (9), respectively.

5. According to the method for real-time positioning of an unmanned vehicle based on geomagnetic / odometer fusion based on adaptive sequential particle filtering described in claim 3, the algorithm flow steps for geomagnetic positioning based on particle filtering are as follows: Establishing the data buffer of the measurement sequence; Get the current magnetometer vector measurement value, attitude angle, and odometer data, calculate the seven-dimensional geomagnetic data components, and add them to the buffer to form a data sequence; Dynamic weight allocation and feature screening based on the stability of each component; Generate particles and calculate particle weights based on odometry data; Calculate the sequence particle filter positioning results; Perform adaptive KLD resampling on particles. If the navigation reaches the end point, it ends. Otherwise, the current magnetometer vector measurement value, attitude angle, and odometer data are re-acquired, and the seven-dimensional geomagnetic data components are calculated and added to the buffer to form a data sequence until the navigation reaches the end point.

6. A real-time positioning system for unmanned vehicles based on geomagnetic / odometer fusion based on adaptive sequential particle filtering, characterized by: The system has a program module corresponding to the steps of any one of claims 1 to 5 above, and executes the steps in the unmanned vehicle geomagnetic / odometer fusion real-time positioning method based on adaptive sequential particle filtering during operation.

7. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores a computer program, and the computer program is configured to implement the steps of a real-time positioning method for unmanned vehicle geomagnetic / odometer fusion based on adaptive sequential particle filtering according to any one of claims 1 to 5 when called by a processor.

Images