A method, program, equipment, and storage medium for underwater matching and localization based on magnetic beacon gradient tensor reference graph meshing and two-step attitude determination.

Abstract

This invention discloses an underwater matching positioning method, program, device, and storage medium based on magnetic beacon gradient tensor reference map gridding and two-step attitude measurement, belonging to the field of underwater navigation and positioning. Based on the known position and attitude of the magnetic beacon in its own coordinate system, the method continuously matches the spatial attitude information of the magnetic beacon coordinate system measured at various points on different reference maps with the known spatial attitude information of the magnetic beacon coordinate system to determine the magnetic beacon in the current domain. A gridding algorithm is used to improve the attitude matching detection accuracy of the reference map grid center point. After determining the magnetic beacon in the current domain, the matching grid center point with the smallest attitude error within the corresponding reference map is selected. The position of the underwater vehicle within the reference map is then determined sequentially. Based on the position of the magnetic beacon and the position of the matching grid center point relative to the magnetic beacon, the absolute position of the underwater vehicle in the geographic coordinate system is determined. This underwater matching positioning can perform underwater magnetic positioning under conditions of large initial errors.

Description

Technical Field

[0001] This invention belongs to the field of underwater navigation and positioning, specifically relating to an underwater matching positioning method, program, device, and storage medium based on magnetic beacon gradient tensor reference graph meshing and two-step attitude measurement. Background Technology

[0002] Currently, underwater navigation primarily relies on inertial navigation technology, supplemented by various other technologies such as underwater acoustic navigation, terrain-matching navigation, geomagnetic navigation, and cooperative navigation. Inertial Navigation Systems (INS), based on Newtonian mechanics, use the specific force and angular velocity information output by accelerometers and gyroscopes to calculate the attitude, velocity, and position of the vehicle. It does not require external signal radiation and is the primary method for autonomous navigation of underwater vehicles. However, due to the existence of inertial drift, navigation errors in INS gradually accumulate, thus maintaining high accuracy only for short periods and requiring periodic correction using other navigation methods. Underwater vehicles cannot receive signals from satellites or other base stations as they can on land or in the air. Furthermore, seawater, being a good conductor, strongly absorbs and scatters electromagnetic waves (such as radio waves), resulting in short propagation distances for radio waves underwater. Therefore, traditional navigation technologies relying on radio waves (such as GPS) are ineffective underwater.

[0003] Acoustic navigation can be divided into Doppler velocimetry (DVL) and underwater acoustic positioning systems. DVL measures the convective velocity of an unmanned underwater vehicle (UUV) using the Doppler frequency shift principle, offering high accuracy, but it suffers from partial beam failure and lacks three-dimensional velocity output. Underwater acoustic positioning systems utilize the time delay and phase difference of underwater sound propagation to determine the relative geometric relationship between the transponder and the UUV. Various noise sources exist in the underwater environment, such as marine life and water temperature conditions, which can interfere with sonar signals and affect the accuracy of acoustic navigation.

[0004] Simultaneous localization and mapping (SLAM) is a method for underwater vehicles (UVs) to use environmental features detected by sensors as navigation references in unknown environments. It estimates the UV's navigation state and constructs feature maps using observations such as range and bearing. While some seabed topography exhibits significant spatial variations, underwater acoustic detection can identify typical seabed features as landmarks to support the SLAM algorithm. However, considering the complexity of marine topography, many seabed features are insufficient for SLAM to identify landmarks, thus limiting its effectiveness. Furthermore, many marine environments lack structured features, making it difficult to obtain clear and defined feature areas; in flat seabed areas, this method may even fail.

[0005] Magnetic field measurements are less affected by the aquatic environment, multipath effects, and signal delays. Their passive detection method makes them easier to implement compared to active detection methods like acoustics and optics. However, traditional geomagnetic navigation relies on measuring the Earth's magnetic field, which is subject to natural variations and local anomalies, potentially leading to low accuracy. This is especially true in complex underwater environments where magnetic field changes are even more difficult to predict, impacting positioning accuracy. Furthermore, geomagnetic navigation performance is significantly affected by environmental factors such as seawater salinity, temperature, and variations in the Earth's magnetic field, all of which can affect the accuracy of magnetic field measurements.

[0006] To address the limitations of existing underwater geomagnetic navigation, a spatially stable magnetic field is established in a localized area by deploying magnetic beacons. This enables underwater matching positioning based on a gridded reference map of the magnetic beacon gradient tensor and two-step attitude determination. This underwater magnetic positioning method has advantages such as all-weather capability, strong concealment, and accuracy that does not accumulate over time, and has the potential to become an integral part of underwater positioning, navigation, and timing systems. Summary of the Invention

[0007] This invention proposes an underwater matching and positioning method based on magnetic beacon gradient tensor reference graph meshing and two-step attitude measurement.

[0008] Several magnetic beacons are deployed in an area where underwater geomagnetic characteristics are not obvious. The spatial locations of these magnetic beacons are known. A coordinate system m is constructed for each magnetic beacon. s The magnetic gradient tensor reference map is generated and stored in the database. Magnetic beacons are appropriately deployed to ensure that the effective signal domains of each beacon do not overlap, and that the attitude information of each beacon coordinate system differs significantly for easy differentiation. When the underwater vehicle reaches the domain of a certain magnetic beacon, the magnetic gradient tensor is measured using the onboard cross magnetic array to obtain the value of the magnetic gradient tensor at the underwater vehicle's current position in the carrier coordinate system b. Magnetic gradient tensor reference maps from the database are sequentially selected, and a meshing algorithm is used to traverse the reference maps to obtain the magnetic gradient tensor value at the m-th position. s The relative attitude between the current magnetic beacon coordinate system and the carrier coordinate system is calculated using a two-step attitude measurement algorithm.

[0009] By combining the underwater vehicle's attitude provided by the inertial navigation system with the relative attitude information calculated by the two-step attitude measurement algorithm, the attitude of the current magnetic beacon coordinate system can be obtained. The error between the current magnetic beacon attitude and the known attitude corresponding to each grid center point is calculated. Grids that meet the error threshold are selected, and the grid data is further meshed to check whether the attitude error of the refined grid converges further. When the error value gradually converges to meet the error requirement, it indicates that the center position of the reference grid used for relative attitude calculation is the correctly matched position of the underwater vehicle. If the attitude error of the refined grid shows a divergent trend, that is, as the grid is gradually refined, it is impossible to find a grid that reduces the attitude error or meets the error threshold, it indicates that the magnetic gradient tensor reference map was selected incorrectly. A new reference map is then used, and the above steps are repeated.

[0010] This invention provides an underwater matching and positioning method based on magnetic beacon gradient tensor reference graph meshing and two-step attitude determination, comprising the following steps:

[0011] Step 1: Deploy several magnetic beacons T with known spatial locations in areas where underwater geomagnetic characteristics are not obvious. s Establish the coordinate system m for each magnetic beacon. s and in m s Construct a reference map of the magnetic gradient tensor within the scope of the system; set the mesh filtering threshold ε. s and attitude measurement error threshold ε;

[0012] Step 2: Measure the magnetic gradient tensor at the current location using the onboard cross-shaped magnetic array. The value of the magnetic gradient tensor in the b-frame is GT. b Initialize the index of the magnetic gradient tensor reference map s = 1; refine the reference map iterative times i = 0;

[0013] Step 3: Transfer the baseline image MT s Each grid center point P in (i) kl The corresponding magnetic gradient tensor in this magnetic beacon system Substituting the two-step attitude measurement algorithm based on the magnetic gradient tensor, where s represents the index of the reference map, (k,l) are the grid coordinates, and i is the number of iterations for refinement, the relative attitude between the carrier coordinate system and the magnetic beacon coordinate system is obtained.

[0014] Step 4: The inertial navigation system outputs a precise attitude matrix. Using the relative attitude of the carrier coordinate system and the relative attitude between the carrier coordinate system and the magnetic beacon coordinate system obtained in Step 3, the relative attitude between the magnetic beacon coordinate system and the geographic coordinate system is obtained. The magnetic gradient tensor reference map is gridded to obtain the vector position of each grid relative to the central magnetic beacon. Then, the attitude error is calculated by combining the relative attitude between the magnetic beacon system and the geographic system corresponding to the current magnetic gradient tensor reference map.

[0015] Step 5: If the attitude error obtained in Step 4 is not greater than the threshold value, then the center point of the grid is the underwater matching and positioning result, and the reference map of the current magnetic beacon's domain is obtained, ending the spatial attitude matching process of the magnetic beacon coordinate system; otherwise, the grid with excessive error is removed.

[0016] If i < I, let i = i + 1, refine the remaining mesh to obtain the magnetic gradient tensor reference map MT1(i), and then use a two-step attitude measurement algorithm combined with an inertial navigation system to calculate the attitude error of the magnetic beacon coordinate system.

[0017] If i = I, that is, the attitude errors calculated by the reference image are all divergent, and s < S, then change the initial value of the next reference image, let s = s + 1, and return to step 2;

[0018] If i = I and s = S, the spatial attitude matching process of the magnetic beacon coordinate system ends, and the underwater matching and positioning is determined to have failed.

[0019] Step 7: Use the relative position and the position of the magnetic beacon obtained by refining the matching with multiple magnetic grids to obtain the matching and positioning results of the underwater vehicle; use the matching and positioning results to perform position feedback correction on the inertial navigation system of the underwater vehicle.

[0020] Furthermore, the two-step attitude measurement algorithm described in step 3 specifically includes the following steps:

[0021] Step 3.1: Based on the magnetic gradient tensor G in the carrier coordinate system b and the magnetic gradient tensor G in the magnetic beacon coordinate system m The attitude matrix Q is calculated, and the attitude angles are obtained.

[0022]

[0023] Where Q1 is the magnetic gradient tensor G in geographic coordinate system. m The eigenvectors are normalized to obtain an orthogonal unit matrix; Q2 is the magnetic gradient tensor G in the carrier coordinate system. b The eigenvectors are normalized to obtain an orthonormal matrix; It is a 3×3 attitude matrix;

[0024] The attitude angle

[0025]

[0026]

[0027] Where, r xy ψ is a vector in the attitude matrix Q; θ is the heading angle; θ is the pitch angle; φ is the roll angle.

[0028] Step 3.2: Utilizing attitude angles The corresponding initial quaternion values ​​q(0) = [q0(0), q1(0), q2(0,)q3(0)] are obtained. Then, the initial quaternion values ​​are substituted into the attitude quaternion equation system to iteratively approximate the attitude measurement algorithm and obtain the accurate relative attitude matrix.

[0029]

[0030] q(k)=f cbm (G b G m ,q(k-1)),k=1,2,3,…

[0031]

[0032] Among them, f cbm The magnetic gradient tensor G is obtained based on measurements under the load system. b And the gradient tensor G in the magnetic beacon system based on the gradient tensor reference graph. m A function to calculate the attitude quaternion between the two coordinate systems;

[0033] Furthermore, the attitude matrix of the relative attitude between the geographic coordinate system and the magnetic beacon coordinate system described in step 4. and attitude angle

[0034]

[0035]

[0036] in, This is the coordinate transformation matrix from the geographic coordinate system to the magnetic beacon coordinate system; This is the coordinate transformation matrix from the magnetic beacon coordinate system to the carrier coordinate system. To output a more accurate geographic-to-vehicle attitude matrix for the inertial navigation system; f cnm The magnetic gradient tensor G is obtained based on measurements under the load system. b The gradient tensor G in the magnetic beacon system based on the gradient tensor reference graph m and the attitude matrix output by the inertial navigation system The function of the attitude angle of the magnetic beacon system relative to the geographic system is calculated.

[0037] Furthermore, step 4 attitude error

[0038]

[0039] Where, Φ s The spatial attitude angle between the magnetic beacon system and the geographic system within the magnetic beacon's current domain.

[0040] Furthermore, step 5 specifically involves: if Then the center point of the grid is discarded directly, and the calculation is performed. like Then output the magnetic beacon number within the effective domain obtained from the current attitude matching. and the position of the grid center point relative to the magnetic beacon Otherwise, let i = i + 1, and refine the mesh of the remaining center points to obtain the baseline map after the i-th iteration of refinement.

[0041] Furthermore, if i reaches the maximum number of iterations for refinement, it indicates that the magnetic gradient tensor reference map is not the magnetic gradient tensor reference map corresponding to the current correct magnetic beacon; at this time, the next magnetic gradient tensor reference map needs to be replaced, that is, let s = s + 1, go to step 2, until s reaches its maximum value.

[0042] The present invention also provides a computer device / equipment / system, including a memory, a processor, and a computer program stored in the memory, wherein the processor executes the computer program to implement the steps of the underwater matching and positioning method based on magnetic beacon gradient tensor reference graph meshing and two-step attitude determination described above.

[0043] The present invention also provides a computer-readable storage medium storing a computer program / instructions thereon, which, when executed by a processor, implements the steps of the underwater matching and positioning method based on magnetic beacon gradient tensor reference graph meshing and two-step attitude determination as described above.

[0044] The present invention also provides a computer program product, including a computer program / instruction that, when executed by a processor, implements the steps of the underwater matching and positioning method based on magnetic beacon gradient tensor reference graph meshing and two-step attitude determination as described above.

[0045] The beneficial effects of this invention are as follows:

[0046] Magnetic beacons are characterized by their ability to operate underwater, underground, or in environments where GPS signals cannot reach, and they are unaffected by the marine environment, meeting the operational environment and conditions required for underwater vehicle position correction. Currently, underwater navigation based on magnetic beacons typically utilizes the magnetic anomalies generated by the beacon to satisfy the Euler homogeneous equation, calculating the beacon's position vector relative to the carrier to achieve single-point relative positioning. However, the aforementioned Euler positioning method is an ill-posed problem and is highly sensitive to observation noise.

[0047] The actual accuracy of the attitude quaternion equation iterative approximation algorithm is generally better than that of the magnetic gradient tensor matrix orthogonal diagonalization attitude measurement algorithm. Furthermore, a good initial solution helps accelerate the iterative solution process of the attitude quaternion nonlinear equations. The two-step attitude measurement algorithm uses the result of the magnetic gradient tensor matrix orthogonal diagonalization algorithm as the initial solution for the attitude quaternion equation iterative approximation algorithm, thus possessing the advantages of fast computation and high accuracy. In addition, the two-step attitude measurement algorithm has low requirements for initial error and can obtain the attitude of the magnetic beacon system even with large initial errors. The iterative gridding of the magnetic gradient tensor reference map can accurately identify the current magnetic beacon using the attitude information of the magnetic beacon system, thereby improving the robustness of this underwater matching positioning. Simultaneously, this underwater matching positioning can also perform underwater magnetic positioning under conditions of large initial errors. Attached Figure Description

[0048] Figure 1 Schematic diagram of underwater navigation and positioning principle based on magnetic beacon system attitude measurement;

[0049] Figure 2 This is a schematic diagram of the magnetic beacon deployment and its functional area gridding;

[0050] Figure 3 This is a detailed schematic diagram of the reference grid for a magnetic beacon.

[0051] Figure 4 This is a flowchart of an underwater matching and localization method based on magnetic beacon gradient tensor reference graph meshing and two-step attitude measurement;

[0052] Figure 5 This is a graph showing the results of calculating the attitude error values ​​by traversing the mesh;

[0053] Figure 6 This is a schematic diagram of the random point location results for a 41×41 grid. Detailed Implementation

[0054] This invention relates to an underwater matching and positioning method based on magnetic beacon gradient tensor reference map gridding and two-step attitude measurement. Using magnetic beacons with known positions and their own coordinate system attitudes, the method continuously matches the spatial attitude information of the magnetic beacon coordinate system measured at various points on different reference maps with the known spatial attitude information of the magnetic beacon coordinate system to determine the magnetic beacon within its current domain. A gridding algorithm is used to improve the attitude matching detection accuracy of the reference map grid center points. After determining the magnetic beacon within the current domain, the matching grid center point with the smallest attitude error within the corresponding reference map is selected. The position of the underwater vehicle within the reference map is then determined sequentially. Finally, based on the position of the magnetic beacon and the position of the matching grid center point relative to the magnetic beacon, the absolute position of the underwater vehicle in the geographic coordinate system is determined.

[0055] Step 1: Deploy underwater magnetic beacons and construct their magnetic gradient tensor reference map.

[0056] Several magnetic beacons T with known spatial locations were deployed in areas where underwater geomagnetic characteristics were not obvious. s To ensure that the effective signal domains of each magnetic beacon do not overlap, establish the self-coordinate system m for each magnetic beacon. s ,s=1,2,…, and in m s The reference map MT of the magnetic gradient tensor within the scope of the system is constructed. s At this time, each magnetic beacon position pBC s and magnetic beacon coordinate system m s Given, and different m s The spatial attitude information of the systems differs significantly, in order to distinguish different magnetic beacons.

[0057] Step 2: Two-step attitude measurement using magnetic gradient tensor and iterative refinement of the reference mesh.

[0058] When the underwater vehicle travels to the effective signal range of a magnetic beacon, the magnetic gradient tensor at its current position is measured in the carrier coordinate system (b) using the onboard cross magnetic array. Following the pre-stored index order of the magnetic gradient tensor reference maps, the initial value MT1(0) of the first magnetic gradient tensor reference map is selected, and the magnetic gradient tensor corresponding to each grid center point is... The relative attitude between the b-frame and the m1-frame is calculated by substituting the two-step attitude measurement algorithm. The spatial attitude of the m1-frame is obtained by combining the attitude matrix output by the inertial navigation system and the calculated relative attitude. The attitude error is calculated by the relative attitude between the magnetic beacon frame and the geographic frame corresponding to the current magnetic gradient tensor reference map. If there is a case where the attitude error is not greater than the threshold value, then the center point of the grid is the underwater matching and positioning result. Otherwise, the grid with excessive error is removed, and the remaining grid is refined to obtain the magnetic gradient tensor reference map MT1(1). The attitude error of the magnetic beacon coordinate system is then calculated by the two-step attitude measurement algorithm. The judgment of the magnetic beacon coordinate system is repeated until the grid is refined to the smallest grid magnetic gradient tensor reference map MT1(I).

[0059] Step 3: Identify the reference map of the current magnetic beacon's area of ​​effect.

[0060] If the attitude errors calculated by the reference map MT1(I) all diverge, it indicates that the magnetic gradient tensor reference map was selected incorrectly. In this case, the initial value of the next reference map MT2(0) is changed, and step 2 is repeated until the attitude error threshold requirement is met, thus obtaining the reference map of the current magnetic beacon's domain. The spatial attitude matching process of the magnetic beacon coordinate system is completed. The position of the matched mesh center point relative to the magnetic beacon is obtained from the matching result. At the same time, magnetic beacons within the effective domain are identified.

[0061] Step 4: Position correction of the underwater vehicle

[0062] Relative positions obtained by multiple mesh refinement matching and the location of the magnetic beacon The matching positioning result pUV of the underwater vehicle is obtained. The pUV is then used to perform position feedback correction on the underwater vehicle's inertial navigation system.

[0063] Example

[0064] An underwater matching and localization method based on magnetic beacon gradient tensor reference map meshing and two-step attitude determination includes the following steps:

[0065] Step 1: Deploy several magnetic beacons T in areas where underwater geomagnetic characteristics are not obvious. s And ensure that the effective signal domains of each magnetic beacon do not overlap, such as Figure 2 As shown. For each magnetic beacon, a separate m is constructed within its scope. s Magnetic gradient tensor reference plot MT under the system s The baseline plot is the magnetic gradient tensor at each grid point within the scope. A grid filtering threshold ε is set. s And the attitude measurement error threshold ε.

[0066] Spatial location of each magnetic beacon pBC s and spatial coordinate system m s Given that the attitude vector Φ is composed of the spatial attitude of each magnetic beacon coordinate system. s The differences between them are significant, so that the different magnetic beacon coordinate systems can be used to distinguish magnetic beacons.

[0067] Step 2: After the underwater vehicle has traveled to the area of ​​influence of a certain magnetic beacon, use the onboard cross magnetic array to determine the value GT of the magnetic gradient tensor at the current position in the b-frame. b And let the index s of the magnetic gradient tensor reference map be 1.

[0068] Step 3: Refine the baseline map by iteration number i = 0, and select the meshed result MT of the s-th magnetic gradient tensor baseline map. s (i).

[0069] Step 4: Transfer the reference image MT s Each grid center point P in (i) kl The corresponding magnetic gradient tensor Substituting the two-step attitude measurement algorithm based on the magnetic gradient tensor, the relative attitude between the current carrier coordinate system and the magnetic beacon coordinate system is calculated.

[0070] The first step of the two-step attitude determination algorithm is an underwater full attitude determination method based on the orthogonal diagonalization of the geomagnetic gradient tensor matrix. The calculation method is shown in Equation (1), based on the magnetic gradient tensor G in the carrier coordinate system. band the magnetic gradient tensor G in the magnetic beacon coordinate system m The attitude matrix Q is calculated, and the attitude angles are obtained:

[0071]

[0072] Where Q1 and Q2 are the tensor matrices G in two different coordinate systems. m G b The eigenvectors are normalized to obtain an orthonormal matrix. It is a 3×3 attitude matrix.

[0073] The attitude angles can be calculated from the above attitude matrix. The calculation method is as follows:

[0074]

[0075] The two-step attitude measurement algorithm based on the magnetic gradient tensor first obtains the relative attitude angle vector by orthogonally diagonalizing the magnetic gradient tensor matrix. This value serves as the initial value for the second step of the two-step attitude measurement algorithm. The initial value of the corresponding quaternion can be calculated using the initial value of the attitude angle. Then, this initial value of the quaternion is substituted into the attitude quaternion equation system to iteratively approximate the attitude measurement algorithm. The calculation principle is shown in equations (2) and (3). q = [q0, q1, q2, q3] is the quaternion used to solve the attitude.

[0076] q(k)=f cbm (G b G m ,q(k-1)), k=1,2,3,… (2)

[0077]

[0078] Among them, f cbm The magnetic gradient tensor G is obtained based on measurements under the load system. b And the gradient tensor G in the magnetic beacon system based on the gradient tensor reference graph. m A function to calculate the attitude quaternion between the two coordinate systems; Let be the transformation matrix between the carrier coordinate system and the magnetic beacon coordinate system, and

[0079] Based on the above formula, a more accurate relative attitude vector can be obtained. Among them G b G m This can be obtained through sensor measurements and pre-stored magnetic beacon reference diagrams. In the above formula, this is equivalent to a constant, thus yielding the functional relationship f(q0,q1,q2,q3)=0, which is simplified below as f cbm(q), the attitude quaternion equation system can be solved iteratively using the Levenberg-Marquardt method, as shown in equation (4):

[0080] Δp=-(J T J+μI) -1 J T f cbm (4)

[0081] Where Δp is the iteration step, and J(q) is the attitude quaternion equation system f cbm The Jacobian matrix of (q), where I is the identity matrix and μ is a positive parameter, is used to prevent J from being... T When J approaches singularity, Δp is too large, and when J T When J is singular, the Gauss-Newton step is undefined, and the positive parameter μ can ensure that equation (4) is always defined, thus improving the stability of the iteration process.

[0082] Step 5: The inertial navigation system outputs a more accurate attitude matrix. Using the spatial attitude of the carrier coordinate system (b-frame) and the attitude of the carrier coordinate system relative to the magnetic beacon coordinate system obtained in step 4, the spatial attitude of the magnetic beacon coordinate system (m-frame) is calculated as follows:

[0083]

[0084] In the formula, It is a coordinate transformation matrix from the geographic coordinate system to the magnetic beacon coordinate system, which contains the spatial attitude information of the magnetic beacon system; This is the coordinate transformation matrix from the magnetic beacon coordinate system to the carrier coordinate system, reflecting the attitude of the carrier coordinate system relative to the magnetic beacon coordinate system. Based on the matrix... Using the relationship between the attitude matrix and attitude angles in equations (1-1) and (1-2), the relative attitude angles between the magnetic beacon system m and the geographic system n can be obtained. The attitude matrix obtained above using the two-step attitude measurement algorithm Attitude matrix obtained from inertial navigation system The process of calculating the attitude angle from geographic system b to magnetic beacon system m is denoted as f. cnm .

[0085] Step 6: Within the domain of a certain magnetic beacon, the magnetic gradient tensor reference map can be gridded to obtain the vector position of each grid relative to the central magnetic beacon. This position is related to the grid coordinates. Based on the grid position, the magnetic beacon attitude and magnetic moment can be used to calculate the relative attitude between the geographic system and the magnetic beacon system at the corresponding grid position, as shown in Equation (6).

[0086]

[0087] Based on the assumption that the spatial attitude angle Φ between the magnetic beacon system and the geographic system is currently located within the magnetic beacon's domain, the carrier is in a position to maintain the magnetic beacon system. s Calculate the attitude error value according to formula (7).

[0088]

[0089] In the formula, The calculated value of the spatial attitude vector representing the magnetic beacon coordinate system is derived from step 5. Obtained through conversion.

[0090] Step 7, if Then the center point of the grid is discarded directly, and the calculation is performed. like Then output the magnetic beacon number within the effective domain obtained from the current attitude matching. and the position of the grid center point relative to the magnetic beacon Otherwise, let i = i + 1, and refine the mesh for the remaining center points as follows: Figure 3 As shown, the baseline map after the i-th iteration of refinement is obtained. Proceed to step 4.

[0091] Step 8: If i reaches the maximum number of iterations for refinement I, it means that the magnetic gradient tensor reference map is not the magnetic gradient tensor reference map corresponding to the current correct magnetic beacon. At this time, it is necessary to replace it with the next magnetic gradient tensor reference map, that is, let s = s + 1, go to step 3, until s reaches its maximum value S.

[0092] Step 9: If the magnetic beacon sequence number exists within the effective domain, then combine... Obtain the matching and positioning result pUV of the underwater vehicle, and use pUV to perform position feedback correction on the underwater vehicle's inertial navigation system; otherwise, the underwater matching and positioning fails.

[0093] The underwater matching and positioning method based on magnetic beacon gradient tensor reference map meshing and two-step attitude measurement involves meshing the magnetic beacon reference map, calculating the attitude error corresponding to the grid center point as a criterion, and traversing each reference map to select the grid center point that meets the criteria for underwater magnetic positioning. This method utilizes a two-step attitude measurement algorithm based on magnetic gradient tensor to efficiently and quickly determine a relatively accurate relative attitude vector of the underwater magnetic beacon. Combined with a relatively accurate attitude matrix output by the inertial navigation system, the spatial attitude of the magnetic beacon coordinate system is determined. Based on the known position and attitude of the magnetic beacon in its own coordinate system, the grid of the magnetic beacon gradient tensor reference map is continuously refined, and the grid center point with the smallest attitude error is calculated and selected, gradually bringing the matching position closer to the actual position of the underwater vehicle, thus completing underwater matching and positioning. The method involved in this invention enables underwater vehicles to navigate and position themselves in underwater areas lacking structured geomagnetic features.

[0094] The two-step attitude measurement algorithm based on the magnetic gradient tensor integrates the advantages of the magnetic gradient tensor matrix orthogonal diagonalization attitude measurement algorithm and the attitude quaternion equation system iterative approximation attitude measurement algorithm. Therefore, the two-step attitude measurement algorithm is characterized by fast computation and applicability under large initial error conditions. Applying the two-step attitude measurement algorithm to the measurement of relative attitude in the magnetic beacon coordinate system can improve its measurement accuracy and speed, which contributes to the robustness and speed of this underwater matching and positioning method.

[0095] Simulation verification was performed based on this method. The magnetic dipole positions were set to [-20; -20; -10], the magnetic moments to [5e7; 1e7; 2e7], the sampling point spacing of the cross array to be 2m, the magnetic gradient tensor reference map plane to be the xy plane, and the relative attitude angle between the magnetic beacon coordinate system and the geographic coordinate system to be [50, 30, 40]. A zero-mean error signal with a standard deviation of 0.1 was superimposed. Traversal simulation verification was performed on 11×11 grids and 21×21 grids, respectively. Figure 5 As shown, the numbers on the X and Y axes represent the corresponding mesh numbers. The upper mesh is 11×11, and the lower mesh is 21×21, with each mesh unit being 1m. The mesh color represents the magnitude of the attitude error value. Here, it is the sum of the cubes of the magnitudes of the differences between the three components of the attitude angle (ψ, θ, φ), simulating the vehicle traveling to each mesh point to calculate the corresponding attitude error value, and performing random point positioning tests within the 41×41 mesh. Figure 6 As shown, the numbers on the horizontal and vertical axes represent the serial numbers corresponding to the refined meshes. Each mesh unit is 1m. The shade of the mesh color represents the magnitude of the attitude error value. Here, it is the sum of the cubes of the magnitudes of the differences of the three components of the attitude angle (ψ, θ, φ). The results verify the feasibility of the method.

[0096] In particular, in some preferred embodiments of the present invention, a computer device is also provided, including a memory and a processor and a computer program stored in the memory, wherein the processor executes the computer program to implement the steps of the underwater matching and positioning method based on magnetic beacon gradient tensor reference map meshing and two-step attitude determination as described in any of the above embodiments.

[0097] In some other preferred embodiments of the present invention, a computer-readable storage medium is also provided, on which a computer program / instruction is stored, wherein when the computer program is executed by a processor, it implements the steps of the underwater matching and positioning method based on magnetic beacon gradient tensor reference map meshing and two-step attitude determination as described in any of the above embodiments.

[0098] Those skilled in the art will understand that all or part of the processes in the above embodiments can be implemented by a computer program instructing related hardware. The computer program can be stored in a non-volatile computer-readable storage medium. When the computer program is executed, it can include the process of an embodiment of an underwater matching and positioning method based on magnetic beacon gradient tensor reference graph meshing and two-step attitude measurement, as described above, which will not be repeated here.

[0099] The above description is merely a preferred embodiment of the present invention and the technical principles employed. Those skilled in the art will understand that the present invention is not limited to the specific embodiments described herein, and various obvious changes, readjustments, and substitutions can be made without departing from the scope of protection of the present invention. Therefore, although the present invention has been described in detail through the above embodiments, the present invention is not limited to the above embodiments, and may include many other equivalent embodiments without departing from the concept of the present invention, the scope of which is determined by the scope of the appended claims.

Claims

1. An underwater matching and positioning method based on magnetic beacon gradient tensor reference graph meshing and two-step attitude determination, characterized in that: Includes the following steps: Step 1: Deploy several magnetic beacons T with known spatial locations in areas where underwater geomagnetic characteristics are not obvious. s Establish the coordinate system m for each magnetic beacon. s and in m s Construct a reference map of the magnetic gradient tensor within the scope of the system; set the mesh filtering threshold ε. s and attitude measurement error threshold ε; Step 2: Measure the value GT of the magnetic gradient tensor at the current location in the b-frame using the onboard cross magnetic array. b Initialize the index s of the magnetic gradient tensor reference map to 1; The baseline graph is refined in iterations of number i = 0; Step 3: Transfer the baseline image MT s Each grid center point P in (i) kl The corresponding magnetic gradient tensor in the magnetic beacon coordinate system Substituting the two-step attitude measurement algorithm based on the magnetic gradient tensor, the relative attitude between the carrier coordinate system and the magnetic beacon coordinate system is obtained; The two-step attitude measurement algorithm specifically includes the following steps: Step 3.1: Based on the magnetic gradient tensor G in the carrier coordinate system b and the magnetic gradient tensor G in the magnetic beacon coordinate system m The attitude matrix Q is calculated, and the attitude angles are obtained. Where Q1 is the magnetic gradient tensor G in the magnetic beacon coordinate system. m The eigenvectors are normalized to obtain an orthogonal unit matrix; Q2 is the magnetic gradient tensor G in the carrier coordinate system. b The eigenvectors are normalized to obtain an orthonormal matrix; It is a 3×3 attitude matrix; The attitude angle Where, r xy ψ is a vector in the attitude matrix Q; θ is the heading angle; θ is the pitch angle; φ is the roll angle. Step 3.2: Utilizing attitude angles The corresponding initial quaternion values ​​q(0) = [q0(0), q1(0), q2(0,)q3(0)] are obtained. Then, the initial quaternion values ​​are substituted into the attitude quaternion equation system to iteratively approximate the attitude measurement algorithm and obtain the accurate relative attitude matrix. q(k)=f cbm (G b ,G m ,q(k-1)),k=1,2,3,… Among them, f cbm For the magnetic gradient tensor G in the carrier coordinate system b and the magnetic gradient tensor G in the magnetic beacon coordinate system m A function to calculate the attitude quaternion between the two coordinate systems; Step 4: The inertial navigation system outputs a precise attitude matrix. Using the relative attitude of the carrier coordinate system and the relative attitude between the carrier coordinate system and the magnetic beacon coordinate system obtained in Step 3, the relative attitude between the magnetic beacon coordinate system and the geographic coordinate system is obtained. The magnetic gradient tensor reference map is gridded to obtain the vector position of each grid relative to the central magnetic beacon. Then, the attitude error is calculated by combining the relative attitude between the magnetic beacon coordinate system and the geographic coordinate system corresponding to the current magnetic gradient tensor reference map. Step 5: If the attitude error obtained in Step 4 is not greater than the threshold value, then the center point of the grid is the underwater matching and positioning result, and the reference map of the current magnetic beacon's domain is obtained, ending the spatial attitude matching process of the magnetic beacon coordinate system; otherwise, the grid with excessive error is removed. If i < I, where I is the maximum number of iterations for refinement, let i = i + 1, refine the remaining mesh to obtain the magnetic gradient tensor reference map MT1(i), and then use a two-step attitude measurement algorithm combined with an inertial navigation system to calculate the attitude error of the magnetic beacon coordinate system. If i = I, meaning the attitude errors calculated from the reference image are all divergent, and s < S, where S is the maximum value of the reference image number, then change the initial value of the next reference image, let s = s + 1, and return to step 2. If i = I and s = S, the spatial attitude matching process of the magnetic beacon coordinate system ends, and the underwater matching and positioning is determined to have failed. Step 7: Use the relative position and the position of the magnetic beacon obtained by refining the matching with multiple magnetic grids to obtain the matching and positioning results of the underwater vehicle; use the matching and positioning results to perform position feedback correction on the inertial navigation system of the underwater vehicle.

2. The underwater matching and positioning method for magnetic beacon gradient tensor reference map meshing and two-step attitude determination according to claim 1, characterized in that: Step 4: Attitude matrix from geographic coordinate system to magnetic beacon coordinate system and attitude angle in, This is the attitude matrix from the geographic coordinate system to the magnetic beacon coordinate system; This is the coordinate transformation matrix from the magnetic beacon coordinate system to the carrier coordinate system. To output a more accurate attitude matrix from the vehicle coordinate system to the geographic coordinate system for the inertial navigation system; f cnm For the magnetic gradient tensor G in the carrier coordinate system b Magnetic gradient tensor G in magnetic beacon coordinate system m and the attitude matrix output by the inertial navigation system The function of calculating the attitude angle of the magnetic beacon coordinate system relative to the geographic coordinate system is obtained.

3. The underwater matching and positioning method for magnetic beacon gradient tensor reference map meshing and two-step attitude determination according to claim 2, characterized in that: Step 4 Attitude Error Where, Φ s The spatial attitude angle between the magnetic beacon coordinate system and the geographic coordinate system within the magnetic beacon's current domain.

4. The underwater matching and positioning method for magnetic beacon gradient tensor reference map meshing and two-step attitude determination according to claim 3, characterized in that: Step 5 specifically involves: If Then the center point of the grid is discarded directly, and the calculation is performed. like Then output the magnetic beacon number within the effective domain obtained from the current attitude matching. and the position of the grid center point relative to the magnetic beacon Otherwise, let i = i + 1, and refine the mesh of the remaining center points to obtain the baseline map after the i-th iteration of refinement.

5. The underwater matching and positioning method for magnetic beacon gradient tensor reference map meshing and two-step attitude determination according to claim 4, characterized in that: If i reaches the maximum number of iterations for refinement, it means that the magnetic gradient tensor reference map is not the magnetic gradient tensor reference map corresponding to the current correct magnetic beacon. At this time, it is necessary to replace the next magnetic gradient tensor reference map, that is, let s = s + 1, go to step 2, until s reaches its maximum value.

6. A computer device / equipment / system, comprising a memory, a processor, and a computer program stored in the memory, characterized in that: The processor executes the computer program to implement the steps of the method according to any one of claims 1 to 5.

7. A computer-readable storage medium having a computer program / instructions stored thereon, characterized in that: When the computer program / instructions are executed by the processor, they implement the steps of the method according to any one of claims 1 to 5.

8. A computer program product comprising a computer program / instructions, characterized in that: When the computer program / instructions are executed by the processor, they implement the steps of the method according to any one of claims 1 to 5.

Images