Underwater matching positioning method based on magnetic beacon gradient tensor reference map gridding and two-step attitude determination, program, equipment and storage medium

Abstract

The invention discloses an underwater matching positioning method based on magnetic beacon gradient tensor reference map gridding and two-step attitude determination, a program, equipment and a storage medium, and belongs to the field of underwater navigation positioning. According to the magnetic beacons with known positions and self coordinate system attitudes, continuously performing matching detection on the space attitude information of the magnetic beacon coordinate system measured by each point of different reference images and the space attitude information of the known magnetic beacon coordinate system, and determining the magnetic beacons in the action range; and the attitude matching detection precision of the grid center point of the reference map is improved through a gridding algorithm. The method comprises the following steps: after determining a magnetic beacon in a current action range, selecting a matched grid center point with the minimum attitude error in a corresponding reference map, sequentially determining the position of the underwater vehicle in the reference map, and determining the absolute position of the underwater vehicle in a geographic coordinate system according to the position of the magnetic beacon and the position of the matched grid center point relative to the magnetic beacon, according to the underwater matching positioning method, underwater magnetic positioning can be carried out under the condition of large initial errors.

Description

Technical Field

[0001] The present invention belongs to the field of underwater navigation and positioning, and specifically relates to an underwater matching positioning method, program, equipment and storage medium based on magnetic beacon gradient tensor reference map gridding and two-step attitude measurement. Background Art

[0002] At present, underwater navigation is mainly based on inertial navigation technology, supplemented by various technologies such as hydroacoustic navigation, terrain matching navigation, geomagnetic navigation and collaborative navigation. The inertial navigation system (INS) is based on the principle of Newtonian mechanics. It uses the specific force and angular velocity information output by the accelerometer and gyroscope to solve the attitude, speed and position information of the carrier. It does not need to radiate signals outward. It is the main way for underwater vehicles to navigate autonomously. However, due to the existence of inertial drift problems, the navigation error of the inertial navigation system will gradually accumulate, so it can only maintain a high accuracy in a short time, and needs to be corrected regularly with the help of other navigation methods. Underwater carriers cannot receive signals from satellites or other base stations like on land or in the air, and seawater, as a good conductor, has a very strong absorption and scattering effect on electromagnetic waves (such as radio waves), resulting in a short distance for radio waves to propagate underwater. Therefore, traditional navigation technologies that rely on radio waves (such as GPS) cannot be effectively used underwater.

[0003] Acoustic navigation can be divided into Doppler velocimeter (DVL) and hydroacoustic positioning system. DVL measures the convective velocity of the unmanned underwater vehicle through the principle of Doppler frequency shift. The velocity measurement accuracy is high, but there is a situation where some beams fail and there is no three-dimensional velocity output; the hydroacoustic positioning system uses the time delay and phase difference of underwater sound propagation to obtain the relative geometric relationship between the transponder and the unmanned underwater vehicle. There are various noise sources in the underwater environment, such as marine life, water temperature conditions, etc., which will interfere with the sonar signal and affect the accuracy of acoustic navigation.

[0004] Underwater simultaneous localization and mapping (SLAM) is a method in which a submersible uses environmental features detected by sensors as a navigation reference in an unknown environment, and uses observations such as distance and azimuth to estimate the navigation state of the submersible and construct a feature map. Some seafloor landforms have obvious spatial changes. Using hydroacoustic detection, typical seafloor landforms can be identified and used as landmark information to support the operation of the SLAM algorithm. Considering the complex terrain environment in the ocean, the ocean landforms in many cases are not sufficient to support the SLAM algorithm to identify feature objects as landmarks, which has certain limitations. There are also many cases where the ocean environment does not have structured features, making it difficult to obtain clear and definite feature areas, and even in flat areas on the seafloor, this method will fail directly.

[0005] Magnetic field measurement is less affected by water environment, multipath effect and signal delay, and its detection method is passive, which makes magnetic field detection easier to implement than active detection methods such as acoustics and optics. However, traditional geomagnetic navigation relies on the measurement of the geomagnetic field, and the geomagnetic field itself has natural changes and local anomalies, which may lead to low navigation accuracy relying on the geomagnetic field. Especially in complex underwater environments, changes in the magnetic field are more difficult to predict, thus affecting the accuracy of positioning. In addition, the performance of geomagnetic navigation is greatly affected by environmental factors, such as the salinity and temperature of seawater and changes in the geomagnetic field, all of which may affect the accuracy of magnetic field measurement.

[0006] In view of the limitations of existing underwater geomagnetic navigation, magnetic beacons are deployed to establish a spatially stable magnetic field in a local area, and underwater matching positioning based on the gridding of the magnetic beacon gradient tensor reference map and two-step attitude measurement is realized. This underwater magnetic positioning method has the advantages of all-weather, strong concealment, and accuracy that does not accumulate over time. It has the potential to become a component of the underwater positioning, navigation and timing system. Summary of the invention

[0007] The present invention proposes an underwater matching positioning method based on magnetic beacon gradient tensor reference map gridding and two-step attitude measurement.

[0008] Several magnetic beacons are deployed in areas where underwater geomagnetic features are not obvious. The spatial positions of these magnetic beacons are known. For each magnetic beacon, its own coordinate system m is constructed. s The magnetic gradient tensor reference map under m is obtained and stored in the database. The magnetic beacons are properly arranged so that the effective signal range of each magnetic beacon does not overlap, and the difference between the attitude information of each magnetic beacon coordinate system is large enough to facilitate distinction. When the underwater vehicle reaches the range of a magnetic beacon, the cross magnetic array on board is used to measure the magnetic gradient tensor to obtain the value of the magnetic gradient tensor at the current position of the underwater vehicle in the carrier coordinate system b. The magnetic gradient tensor reference map in the database is selected sequentially, and the gridding algorithm is used to traverse the reference map to obtain the magnetic gradient tensor in m. 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] The attitude of the current magnetic beacon coordinate system can be obtained by combining the attitude of the underwater vehicle provided by the inertial navigation system and the relative attitude information calculated by the two-step attitude measurement algorithm; the error between the current magnetic beacon attitude corresponding to each grid center point and the known attitude is calculated, the grids that meet the error threshold are screened out, and the grid data is gridded again to check whether the attitude error of the refined grid is further converged. When the error value gradually converges to meet the error requirement, it indicates that the center position of the reference map 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 makes the attitude error smaller or meets the error threshold, indicating that the magnetic gradient tensor reference map is selected incorrectly, replace the next reference map and repeat the above steps.

[0010] The present invention provides an underwater matching positioning method based on magnetic beacon gradient tensor reference map gridding and two-step attitude measurement, comprising the following steps:

[0011] Step 1: Deploy several magnetic beacons with known spatial positions in areas where underwater geomagnetic features are not obvious. s , establish the own coordinate system m of each magnetic beacon s , and in m s Construct a magnetic gradient tensor reference map within the scope of the system; set the grid screening threshold value ε s and attitude error threshold ε;

[0012] Step 2: Use the cross magnetic array to measure the magnetic gradient tensor at the current position. The value of the magnetic gradient tensor in the b system GT b , initialize the sequence number of the magnetic gradient tensor reference map s = 1; the number of iterative refinements of the reference map i = 0;

[0013] Step 3: MT the reference image s Each grid center point P in (i) kl The corresponding magnetic gradient tensor in the magnetic beacon system is Substitute the two-step attitude measurement algorithm based on the magnetic gradient tensor, where s represents the serial number of the reference image, (k, l) is the grid coordinate, and i is the number of iterative refinements, and then obtain the relative attitude between the carrier coordinate system and the magnetic beacon coordinate system;

[0014] Step 4: The inertial navigation system outputs an accurate attitude matrix. The relative attitude between the magnetic beacon coordinate system and the geographic coordinate system is obtained by 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 magnetic gradient tensor reference map is gridded to obtain the vector position of each grid relative to the magnetic beacon at the center position, and 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, the center point of the grid is the underwater matching positioning result, and the reference map of the current magnetic beacon scope is obtained, and the spatial attitude matching process of the magnetic beacon coordinate system is ended; otherwise, the grid with excessive error is removed;

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

[0017] If i=I, that is, the posture errors calculated by the reference image are all divergent, and s<S, then replace the initial value of the next reference image, set 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 is terminated, and it is determined that the underwater matching positioning has failed;

[0019] Step 7: Use the relative position obtained by multi-magnetic grid refinement matching and the position of the magnetic beacon to obtain the matching positioning result of the underwater vehicle; use the matching positioning result to perform position feedback correction on the inertial navigation system of the underwater vehicle.

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

[0021] Step 3.1: Magnetic gradient tensor G based on the carrier coordinate system b And the magnetic gradient tensor G in the magnetic beacon coordinate system m Calculate the attitude matrix Q and get the attitude angle

[0022]

[0023] Where Q1 is the magnetic gradient tensor G in the geographic coordinate system m The unit orthogonal matrix is ​​obtained by normalizing the eigenvector of b The unit orthogonal matrix obtained by normalizing the eigenvectors of is a 3×3 posture matrix;

[0024] The attitude angle

[0025]

[0026]

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

[0028] Step 3.2: Using the attitude angle Get the corresponding quaternion initial value q(0) = [q0(0), q1(0), q2(0,)q3(0)], and then substitute the quaternion initial value into the iterative approximation attitude measurement algorithm of the attitude quaternion equation group to 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 is the magnetic gradient tensor G obtained based on the measurement under load b And the gradient tensor G in the magnetic beacon system provided by the gradient tensor reference map m Calculate the function of the attitude quaternion between two coordinate systems;

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

[0034]

[0035]

[0036] in, is the coordinate transformation matrix from the geographic coordinate system to the magnetic beacon coordinate system; is the coordinate transformation matrix from the magnetic beacon coordinate system to the carrier coordinate system, Output a more accurate attitude matrix from the geographic system to the carrier system for the inertial navigation system; cnm is the magnetic gradient tensor G obtained based on the measurement under load b , based on the gradient tensor reference map provided by the magnetic beacon system, the gradient tensor G m And the attitude matrix based on the inertial navigation system output Calculate the function of the attitude angle of the magnetic beacon system relative to the geographic system.

[0037] Furthermore, step 4 posture error

[0038]

[0039] Among them, Φ s It is the spatial attitude angle between the corresponding magnetic beacon system and the geographic system within the scope of the magnetic beacon where the carrier is currently located.

[0040] Furthermore, the step 5 is specifically as follows: Then the center point of the grid is directly discarded, and the calculation like Then output the magnetic beacon serial number within the effective scope obtained by matching the current posture and the location of the grid center point relative to the magnetic beacon Otherwise, let i = i + 1, refine the grid of the remaining center points, and obtain the benchmark image after the i-th iteration refinement

[0041] Furthermore, if i reaches the maximum number of iterative refinements, 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, the next magnetic gradient tensor reference map needs to be replaced, that is, let s = s + 1, and go to step 2 until s reaches its maximum value.

[0042] The present invention also provides a computer device / equipment / system, comprising a memory, a processor and a computer program stored in the memory, wherein when the processor executes the computer program, the steps of the underwater matching positioning method based on gridding of magnetic beacon gradient tensor reference map and two-step attitude measurement as described in any one of the above items are implemented.

[0043] The present invention also provides a computer-readable storage medium having a computer program / instruction stored thereon. When the computer program / instruction is executed by a processor, the steps of the underwater matching positioning method based on gridding of a magnetic beacon gradient tensor reference map and two-step attitude measurement as described in any of the above items are implemented.

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

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

[0046] Magnetic beacons can work underwater, underground or in other environments where GPS signals cannot reach, and are not affected by the marine environment, meeting the working environment and conditions for underwater vehicle position correction. Currently, underwater navigation based on magnetic beacons usually uses the magnetic anomaly generated by the magnetic beacon to satisfy the Euler homogeneous equation relationship to calculate the position vector of the magnetic beacon relative to the carrier to achieve single-point relative positioning. However, the above-mentioned Euler positioning method is an ill-posed problem and is very sensitive to observation noise.

[0047] The actual accuracy of the iterative approximation attitude measurement algorithm of the attitude quaternion equation group is usually better than that of the orthogonal diagonalization attitude measurement algorithm of the magnetic gradient tensor matrix. At the same time, a good initial solution helps to speed up the iterative solution process of the attitude quaternion nonlinear equation group. The two-step attitude measurement algorithm uses the result of the orthogonal diagonalization attitude measurement algorithm of the magnetic gradient tensor matrix as the initial solution of the iterative approximation attitude measurement algorithm of the attitude quaternion equation group, so it has the advantages of fast calculation and high accuracy. In addition, the two-step attitude measurement algorithm has low requirements on the initial error and can obtain the attitude of the magnetic beacon system when the initial error is large; the iterative gridding of the magnetic gradient tensor reference map can use the attitude information of the magnetic beacon system to accurately identify the current magnetic beacon, thereby improving the robustness of this underwater matching positioning. At the same time, this underwater matching positioning can also perform underwater magnetic positioning under the condition of large initial error. BRIEF DESCRIPTION OF THE DRAWINGS

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

[0049] Figure 2 It is a grid diagram of the layout and scope of magnetic beacons;

[0050] Figure 3 It is a schematic diagram of the grid refinement of the reference map of the magnetic beacon;

[0051] Figure 4 It is a flow chart of an underwater matching positioning method based on the gridding of magnetic beacon gradient tensor reference map and two-step attitude measurement;

[0052] Figure 5 It is the result graph of traversing the grid to calculate the attitude error value;

[0053] Figure 6 This is a schematic diagram of the positioning results of random points in a 41×41 grid. DETAILED DESCRIPTION

[0054] The present invention relates to an underwater matching positioning method based on magnetic beacon gradient tensor reference map gridding and two-step attitude measurement. According to a magnetic beacon whose position and its own coordinate system attitude are known, the magnetic beacon in the scope is determined by continuously matching and detecting the spatial attitude information of the magnetic beacon coordinate system measured at each point of different reference maps with the spatial attitude information of the known magnetic beacon coordinate system, and the attitude matching detection accuracy of the center point of the grid of the reference map is improved through a gridding algorithm. After determining the magnetic beacon in the current scope, the center point of the matching grid with the smallest attitude error in the corresponding reference map is selected, and the position of the underwater vehicle in the reference map is determined in turn, and the absolute position of the underwater vehicle in the geographic coordinate system is determined based on the position of the magnetic beacon and the position of the matching grid center point relative to the magnetic beacon.

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

[0056] In the area where the underwater geomagnetic features are not obvious, several magnetic beacons with known spatial positions are deployed. s , ensure that the effective signal range of each magnetic beacon does not overlap, and establish the own coordinate system m of each magnetic beacon s ,s=1,2,…, and in m s The magnetic gradient tensor benchmark map MT is constructed within the scope of the system s At this time, each magnetic beacon position pBC s and magnetic beacon coordinate system m s Known and different m s The spatial attitude information between the systems is quite different, so it is easier to distinguish different magnetic beacons.

[0057] Step 2: Two-step attitude measurement of magnetic gradient tensor and refinement of reference map grid

[0058] When the underwater vehicle is within the effective signal range of a magnetic beacon, the cross magnetic array on board is used to measure the value of the magnetic gradient tensor at the current position in the carrier coordinate system (b). According to the order of the pre-stored magnetic gradient tensor reference map corners, 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 Substitute into the two-step attitude measurement algorithm to calculate the relative attitude between the b system and the m1 system. Combine the attitude matrix output by the inertial navigation system and the calculated relative attitude to obtain the spatial attitude of the m1 system, and the relative attitude between the magnetic beacon system and the geographic system corresponding to the current magnetic gradient tensor reference map to calculate the attitude error. If there is a situation where the attitude error is not greater than the threshold value, the center point of the grid is the underwater matching positioning result. Otherwise, remove the grids with excessive errors, refine the remaining grids to obtain the magnetic gradient tensor reference map MT1(1), and then use the two-step attitude measurement algorithm to calculate the attitude error of the magnetic beacon coordinate system; repeat the judgment of the magnetic beacon coordinate system until the grid is refined to the magnetic gradient tensor reference map MT1(I) of the minimum grid.

[0059] Step 3: Identify the reference map of the current magnetic beacon scope

[0060] If the attitude errors calculated by the reference map MT1(I) all diverge, it means that the magnetic gradient tensor reference map is incorrectly selected. Then replace the initial value MT2(0) of the next reference map and repeat step 2 until the attitude error threshold requirement is met. The reference map of the current magnetic beacon is obtained. End the spatial attitude matching process of the magnetic beacon coordinate system. The matching result is used to obtain the position of the center point of the matching grid relative to the magnetic beacon. At the same time, magnetic beacons within the effective range are identified.

[0061] Step 4: Position correction of underwater vehicle

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

[0063] Example

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

[0065] Step 1: Deploy several magnetic beacons in areas where underwater geomagnetic features are not obvious. s , and ensure that the effective signal range of each magnetic beacon does not overlap, such as Figure 2 As shown. For each magnetic beacon scope, construct its own m s Magnetic Gradient Tensor Reference Map MT s , the reference image is the magnetic gradient tensor at each grid point within the scope. Set the grid screening threshold value ε s and attitude error threshold ε.

[0066] The spatial position pBC of each magnetic beacon s and the spatial coordinate system m s It is known that the attitude vector Φ composed of the spatial attitude of each magnetic beacon coordinate system s The difference between them is large, so that the magnetic beacons can be distinguished by using the different magnetic beacon coordinate systems.

[0067] Step 2: When the underwater vehicle moves into the range of a magnetic beacon, the value of the magnetic gradient tensor in the b system GT at the current position is measured using the cross magnetic array carried by the vehicle. b , and let the serial number of the magnetic gradient tensor reference map s=1.

[0068] Step 3, iterative refinement of the reference map i = 0, select the gridding result MT of the sth magnetic gradient tensor reference map s (i).

[0069] Step 4: MT the base map s Each grid center point P in (i) kl The corresponding magnetic gradient tensor Substitute the two-step attitude measurement algorithm based on the magnetic gradient tensor to calculate the relative attitude between the current carrier coordinate system and the magnetic beacon coordinate system.

[0070] The first step of the two-step attitude measurement algorithm is the underwater full attitude determination method based on the orthogonal diagonalization of the geomagnetic gradient tensor matrix. The calculation method is shown in formula (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 Calculate the attitude matrix Q and get the attitude angle:

[0071]

[0072] Where Q1 and Q2 are respectively the tensor matrices G in two different coordinate systems. m ,G b The unit orthogonal matrix obtained by normalizing the eigenvectors of is a 3×3 posture matrix.

[0073] According to the above attitude matrix, the attitude angle can be calculated The calculation is as follows:

[0074]

[0075] The two-step attitude measurement algorithm based on the magnetic gradient tensor is to first obtain the relative attitude angle vector by orthogonal diagonalizing the magnetic gradient tensor matrix. This value is used as the initial value of the second step of the two-step attitude measurement algorithm. The initial value of the attitude angle can be used to calculate the corresponding quaternion initial value, and then this quaternion initial value is substituted into the attitude quaternion equation group iterative approximation attitude measurement algorithm. The calculation principle is shown in equation (2) and equation (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 is the magnetic gradient tensor G obtained based on the measurement under load b And the gradient tensor G in the magnetic beacon system provided by the gradient tensor reference map m Calculate the function of the attitude quaternion between two coordinate systems; is 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 It can be obtained through sensor measurement and pre-stored magnetic beacon system reference map, which is equivalent to a constant in the above formula, and then the functional relationship of f(q0,q1,q2,q3)=0 can be obtained, which is briefly expressed as f cbm(q), the attitude quaternion equations can be iteratively solved 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, J(q) is the attitude quaternion equation system f cbm The Jacobian matrix of (q), I is the identity matrix, μ is a positive parameter, in order to prevent J T When J is close to singularity, Δp is too large, and when J T When J is singular, the Gauss-Newton step is not defined. 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 The spatial attitude of the carrier coordinate system (b system) and the attitude of the carrier coordinate system relative to the magnetic beacon coordinate system obtained in step 4 are used to calculate the spatial attitude of the magnetic beacon coordinate system (m system). The calculation formula is as follows:

[0083]

[0084] In the formula, is the 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; is the coordinate transformation matrix from the magnetic beacon coordinate system to the carrier coordinate system, which reflects the posture of the carrier coordinate system relative to the magnetic beacon coordinate system. Using the relationship between the attitude matrix and the attitude angle in equations (1-1) and (1-2), the relative attitude angle between the magnetic beacon system m and the geographic system n can be obtained: The above posture matrix obtained by the two-step posture measurement algorithm is And the attitude matrix obtained by the inertial navigation system The calculation process of obtaining the attitude angle from the geographic system b to the magnetic beacon system m is recorded as f cnm .

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

[0086]

[0087] According to the assumption that the carrier is currently in the range of the magnetic beacon, the spatial attitude angle Φ between the magnetic beacon system and the geographic 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 obtained from step 5 Converted.

[0090] Step 7: If Then the center point of the grid is directly discarded, and the calculation like Then output the magnetic beacon serial number within the effective scope obtained by matching the current posture and the location of the grid center point relative to the magnetic beacon Otherwise, let i = i + 1, and refine the grid of the remaining center points. The grid refinement method is as follows: Figure 3 As shown, the benchmark image after the i-th iteration refinement is obtained Go to step 4.

[0091] Step 8: If i reaches the maximum number of iterative refinements 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, the next magnetic gradient tensor reference map needs to be replaced, that is, let s = s + 1, and go to step 3 until s reaches its maximum value S.

[0092] Step 9: If the magnetic beacon number exists within the valid scope, combine The matching positioning result pUV of the underwater vehicle is obtained, and the pUV is used to perform position feedback correction on the inertial navigation system of the underwater vehicle; otherwise, the underwater matching positioning fails.

[0093] The underwater matching positioning method based on the gridding of the magnetic beacon gradient tensor reference map and two-step attitude measurement realizes underwater magnetic positioning by gridding the magnetic beacon reference map, calculating the attitude error corresponding to the grid center point as the discrimination condition, traversing each reference map to screen out the grid center point that meets the conditions. The method uses a two-step attitude measurement algorithm based on the magnetic gradient tensor to efficiently and quickly determine the more accurate relative attitude vector of the underwater magnetic beacon; combined with the output of a more accurate attitude matrix of the inertial navigation system to determine the spatial attitude of the magnetic beacon coordinate system, according to the magnetic beacon whose position and its own coordinate system attitude are known, by continuously refining the grid of the magnetic beacon gradient tensor reference map, calculating and screening the grid center point with the smallest attitude error, so that the matching position gradually approaches the real position point of the underwater vehicle, and completes the underwater matching positioning. The method involved in the present invention can realize the function of navigation and positioning of underwater vehicles in underwater areas without structured geomagnetic features.

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

[0095] Based on this method, simulation verification is performed, setting the magnetic dipole position to [-20; -20; -10], the magnetic moment to [5e7; 1e7; 2e7], the cross array sampling point spacing to 2m, the magnetic gradient tensor reference plane to the xy plane, the relative attitude angle between the magnetic beacon coordinate system and the geographic coordinate system to [50, 30, 40], and superimposing a zero-mean error signal with a standard deviation of 0.1. Traversal simulation verification is performed on 11×11 grids and 21×21 grids, such as Figure 5 As shown, the numbers on the X and Y axes are the serial numbers corresponding to the refined grids. The upper grid is 11×11, and the lower grid is 21×21. Each grid unit is 1m. The grid color is the size of the attitude error value. Here, it is the cube of the modulus of the difference between the three components of the attitude angle (ψ, θ, φ) and the corresponding attitude error value calculated by simulating the vehicle driving to each grid point, as well as the positioning test of random points in the 41×41 grid. Figure 6 As shown, the numbers on the horizontal and vertical axes represent the serial numbers of the refined grids, each grid unit is 1m, and the color of the grid represents the size of the attitude error value, which is the cube sum of the modulus of the difference between the three components of the attitude angle (ψ, θ, φ). The results verify the feasibility of this 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, and when the processor executes the computer program, the steps of an underwater matching positioning method based on gridding of a magnetic beacon gradient tensor reference map and two-step attitude measurement as described in any of the above embodiments are implemented.

[0097] In other preferred embodiments of the present invention, a computer-readable storage medium is provided, on which a computer program / instructions are stored. When the computer program is executed by a processor, the steps of an underwater matching positioning method based on gridding of a magnetic beacon gradient tensor reference map and two-step attitude measurement described in any of the above embodiments are implemented.

[0098] A person of ordinary skill in the art can understand that all or part of the processes in the above-mentioned embodiment method can be implemented by instructing related hardware through a computer program. 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 the above-mentioned embodiment of the underwater matching positioning method based on the gridding of the magnetic beacon gradient tensor reference map and the two-step attitude measurement, which will not be repeated here.

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

Claims

1. An underwater matching positioning method based on magnetic beacon gradient tensor reference map gridding and two-step attitude measurement, characterized in that: The following steps are involved: Step 1: Deploy several magnetic beacons with known spatial positions in areas where underwater geomagnetic features are not obvious. s , establish the coordinate system m of each magnetic beacon s , and in m s Construct a magnetic gradient tensor reference map within the scope of the system; set the grid screening threshold value ε s and attitude error threshold ε; Step 2: Use the cross magnetic array to measure the magnetic gradient tensor at the current position. The value of the magnetic gradient tensor in the b system GT b , initialize the sequence number s=1 of the magnetic gradient tensor reference map; The number of iterative refinements of the benchmark graph is i=0; Step 3: MT the reference image s Each grid center point P in (i) kl The corresponding magnetic gradient tensor in the magnetic beacon system is Substitute the two-step attitude measurement algorithm based on the magnetic gradient tensor to obtain the relative attitude between the carrier coordinate system and the magnetic beacon coordinate system; Step 4: The inertial navigation system outputs an accurate attitude matrix. The relative attitude between the magnetic beacon coordinate system and the geographic coordinate system is obtained by 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 magnetic gradient tensor reference map is gridded to obtain the vector position of each grid relative to the magnetic beacon at the center position, and 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. Step 5: If the attitude error obtained in step 4 is not greater than the threshold value, the center point of the grid is the underwater matching positioning result, and the reference map of the current magnetic beacon scope is obtained, and the spatial attitude matching process of the magnetic beacon coordinate system is ended; Otherwise, remove the grids with too large errors; If i<I, let i=i+1, refine the remaining grids to obtain the magnetic gradient tensor reference map MT1(i), and then calculate the attitude error of the magnetic beacon coordinate system by combining the two-step attitude measurement algorithm with the inertial navigation system; If i=I, that is, the posture errors calculated by the reference image are all divergent, and s<S, then replace the initial value of the next reference image, set 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 is terminated, and it is determined that the underwater matching positioning has failed; Step 7: Use the relative position obtained by multi-magnetic grid refinement matching and the position of the magnetic beacon to obtain the matching positioning result of the underwater vehicle; use the matching positioning result to perform position feedback correction on the inertial navigation system of the underwater vehicle.

2. The underwater matching positioning method of magnetic beacon gradient tensor reference map gridding and two-step attitude measurement according to claim 1 is characterized in that: The two-step attitude measurement algorithm described in step 3 specifically includes the following steps: Step 3.1: Magnetic gradient tensor G based on the carrier coordinate system b And the magnetic gradient tensor G in the magnetic beacon coordinate system m Calculate the attitude matrix Q and get the attitude angle Where Q1 is the magnetic gradient tensor G in the geographic coordinate system m The unit orthogonal matrix is ​​obtained by normalizing the eigenvector of b The unit orthogonal matrix obtained by normalizing the eigenvectors of is a 3×3 posture matrix; The attitude angle Φ0MT s (k,l) (i) Among them, r xy is the vector in the attitude matrix Q; ψ is the heading angle; θ is the pitch angle; φ is the roll angle; Step 3.2: Using the attitude angle Φ0MT s (k,l) (i) Obtain the corresponding quaternion initial value q(0) = [q0(0), q1(0), q2(0,)q3(0)], and then substitute the quaternion initial value into the iterative approximation attitude measurement algorithm of the attitude quaternion equation group to 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 is the magnetic gradient tensor G obtained based on the measurement under load b And the gradient tensor G in the magnetic beacon system provided by the gradient tensor reference map m Calculate the function of the attitude quaternion between two coordinate systems.

3. The underwater matching positioning method of magnetic beacon gradient tensor reference map gridding and two-step attitude measurement according to claim 2 is characterized in that: Step 4: The attitude matrix of the relative attitude between the geographic coordinate system and the magnetic beacon coordinate system and attitude angle in, is the coordinate transformation matrix from the geographic coordinate system to the magnetic beacon coordinate system; is the coordinate transformation matrix from the magnetic beacon coordinate system to the carrier coordinate system, Output a more accurate attitude matrix from the carrier system to the geographic system for the inertial navigation system; cnm is the magnetic gradient tensor C obtained based on the measurement under load b , based on the gradient tensor reference map provided by the magnetic beacon system, the gradient tensor G m And the attitude matrix based on the inertial navigation system output Calculate the function of the attitude angle of the magnetic beacon system relative to the geographic system.

4. The underwater matching positioning method of magnetic beacon gradient tensor reference map gridding and two-step attitude measurement according to claim 3 is characterized by: Step 4: Attitude Error Among them, Φ s It is the spatial attitude angle between the corresponding magnetic beacon system and the geographic system within the scope of the magnetic beacon where the carrier is currently located.

5. The underwater matching positioning method of magnetic beacon gradient tensor reference map gridding and two-step attitude measurement according to claim 4 is characterized in that: The step 5 is specifically as follows: Then the center point of the grid is directly discarded, and the calculation like Then output the magnetic beacon serial number within the effective scope obtained by matching the current posture and the location of the grid center point relative to the magnetic beacon Otherwise, let i = i + 1, refine the grid of the remaining center points, and obtain the benchmark image after the i-th iteration refinement 6. The underwater matching positioning method of magnetic beacon gradient tensor reference map gridding and two-step attitude measurement according to claim 5, characterized in that: If i reaches the maximum number of iterative refinements, 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, the next magnetic gradient tensor reference map needs to be replaced, that is, let s = s + 1, and go to step 2 until s reaches its maximum value.

7. 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 6.

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

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

Images