A seabed topography assisted navigation method, program, device, and storage medium

Abstract

The present application belongs to the technical field of underwater navigation and positioning, and particularly relates to a seabed terrain aided navigation method, program, device and storage medium. The present application introduces the constraint zonotope in the particle filter into the seabed terrain aided navigation method, reduces the running consumption of the algorithm, and can process the measurement noise of the low-resolution prior sea chart unknown distribution. The present application uses the contour lines and grid points in the prior sea chart to extract suitable water depth data, and performs the outer packaging on the water depth data through the constraint zonotope, so that the real water depth data is in the constraint zonotope, the robustness of the navigation method is improved without increasing the system calculation consumption; the navigation result effectiveness evaluation is performed through the geometric position center water depth difference and the volume of the constraint zonotope, and the feasibility of the navigation method in the actual application is improved.

Description

Technical Field

[0001] The present invention belongs to the field of underwater navigation and positioning technology, and specifically relates to a method, program, equipment and storage medium for auxiliary navigation based on seabed terrain. Background Art

[0002] With the increasing pace of ocean exploration and development, higher requirements are being placed on the accuracy and convenience of underwater positioning. Traditional inertial navigation systems cannot provide accurate navigation results for long-term underwater navigation due to the cumulative errors that increase over time. Acoustic navigation requires the deployment and recovery of underwater acoustic arrays, which is expensive and time-consuming. Underwater geophysical field-assisted navigation methods do not require external sensor assistance and do not have cumulative errors, which has the potential to achieve long-distance underwater navigation. Among underwater geophysical field-assisted navigation methods, terrain-assisted navigation methods are the most commonly used geophysical field-assisted navigation methods, as they provide accurate data measurement and convenient prior chart measurement.

[0003] The seabed terrain-assisted navigation method that uses single-beam sonar as a data acquisition method and relies on low-resolution prior nautical charts is one of the main development directions of terrain-assisted navigation methods. However, due to the limited measurement information of single-beam sonar, high requirements are placed on the robustness of the seabed terrain-assisted navigation method. In the current seabed terrain-assisted navigation filtering method, particle filtering uses a large number of particles to fit the real distribution and can obtain robust positioning results. However, a large number of particles will cause huge computational consumption and it is difficult to run on a low-cost host. At the same time, particle filtering assumes that the measurement noise of the low-resolution prior nautical chart obeys a Gaussian distribution, but the measurement noise of the low-resolution prior nautical chart is of unknown distribution, which makes it difficult to obtain robust and efficient positioning results in the seabed terrain-assisted navigation method.

[0004] A patent application, published on January 21, 2022, with publication number CN113959448A and titled "An Underwater Terrain-Assisted Navigation Method Based on an Improved Salp Swarm Algorithm," utilizes an improved salp swarm algorithm to locate sub-images within a digital seafloor topographic map. However, the invention utilizes a multi-beam sonar sensor, failing to factor in the information-poor measurements of single-beam sonar into the navigation algorithm. Furthermore, the invention fails to address the low resolution of prior nautical charts.

[0005] A patent application, published on January 3, 2023, with publication number CN115560765A and titled "A Terrain-Assisted Navigation Method for AUVs Based on Window Sliding," utilizes inertial navigation information for terrain-assisted navigation, gradually moving the window along the entire path to achieve positioning. However, this method fails to consider the computational overhead of the navigation algorithm and fails to factor in the measurement noise of the unknown distribution of low-resolution prior nautical charts. SUMMARY

[0006] The present application aims to solve the problem that the current single-beam sonar seabed terrain aided navigation method has large calculation consumption and is difficult to handle unknown measurement noise distribution due to the particle filtering method, and provides a seabed terrain aided navigation method, program, device and storage medium.

[0007] A seabed terrain aided navigation method, comprising the following steps:

[0008] Step 1: At the initial moment, the horizontal position coordinates (x * ,y * ) and error range of the underwater vehicle are obtained according to the inertial navigation system carried by the underwater vehicle; under the premise that the horizontal position coordinates of the underwater vehicle within the error range are all included, a constraint totally symmetric polytope with the minimum corresponding volume is constructed with (x * ,y * ) as the center

[0009] Step 2: The motion vector of the underwater vehicle from the last time to the current time is obtained according to the inertial navigation system, the center position and generating vector of the constraint totally symmetric polytope at the last time are updated, and the motion updated constraint totally symmetric polytope is obtained

[0010] Step 3: The water depth measurement value of the position of the underwater vehicle at the current time is obtained according to the single-beam sonar and depth gauge carried by the underwater vehicle

[0011] Step 4: In the prior chart, a search radius is set, the search range is determined with (x * ,y * ) as the center, and the real position of the underwater vehicle is ensured to be within the search range

[0012] Step 5: Within the search range of the prior chart, considering the error of the water depth measurement value, all the isobaths within the error range are extracted, and the extracted isobaths are discretized into isobath points; the center points of the grids within the error range are extracted by using grid search

[0013] Step 6: The constraint totally symmetric polytope is wrapped outside the extracted isobath points and grid center points, and under the premise that all the isobath points and grid center points are included, a constraint totally symmetric polytope with the minimum corresponding volume is constructed

[0014] Step 7: The intersection of is calculated The The geometric position center coordinates are used as the estimated horizontal position coordinates of the underwater vehicle at the current moment;

[0015] Step 8: Obtain the water depth value at the current moment of the estimated horizontal position coordinate value of the underwater vehicle in the priori nautical chart, calculate the error between the water depth value and the water depth measurement value; and compare the error with the water depth value. The volume of is input into the validity evaluation function to obtain the validity evaluation result of the navigation result; if the navigation result at the current moment is determined to be valid, the horizontal position coordinate estimate of the underwater vehicle at the current moment is fed back to the inertial navigation system for correction;

[0016] Step 9: If the underwater vehicle has not completed the mission, return to step 2 and continue navigation.

[0017] Furthermore, the step 2 is specifically as follows:

[0018]

[0019] Among them, u t-1 Represents the motion vector of the underwater vehicle from the previous moment to the current moment provided by the inertial navigation system; Represents the constrained fully symmetric polytope corresponding to the motion noise.

[0020] Furthermore, the method for setting the search radius in step 4 is:

[0021] Pre-calculate the terrain relief of each area in the prior nautical chart, and normalize the values ​​of the terrain relief of all areas to the range of 0.5-1;

[0022] According to the error range of the inertial navigation system at the current moment obtained in step 1, the initial value R0 of the search radius is determined. According to (x * ,y * ) is the normalized value of the terrain relief in the area The search radius R is set to:

[0023] Furthermore, the terrain relief is calculated as follows:

[0024]

[0025] Where M and N are the length and width of the calculation area respectively; h(i,j) represents the water depth data at position (i,j) in the prior nautical chart.

[0026] Furthermore, the calculation in step 7 is and intersection of Specifically:

[0027] The constrained fully symmetric polytope is constructed as follows:

[0028] Z=<c,G,A,b>

[0029] Where c represents the center of the constrained fully symmetric polytope Z, G is the generating vector, and A and b are the constraints;

[0030] and intersection of The calculation method is:

[0031]

[0032] Where R is a matrix that satisfies

[0033] Furthermore, the method for obtaining the effectiveness evaluation function in step 8 is:

[0034] A training data set is constructed, and the difference between the water depth values ​​and the volume of the constrained fully symmetric polyhedron is used as the input of the twin support vector machine, and the navigation error is used as the output of the twin support vector machine. After training, the regression relationship between the difference between the volume of the constrained fully symmetric polyhedron and the water depth values ​​and the validity of the navigation results is obtained. The regression function is the validity evaluation function.

[0035] Furthermore, the constrained fully symmetric polytope obtained in step 7 is After that, the dimension and complexity are reduced to reduce the computational cost of the algorithm.

[0036] A computer device / equipment / system includes 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 above-mentioned seabed terrain-based assisted navigation method.

[0037] A computer-readable storage medium stores a computer program / instruction, which, when executed by a processor, implements the steps of the above-mentioned seabed terrain-based assisted navigation method.

[0038] A computer program product includes a computer program / instruction, which implements the steps of the above-mentioned seabed terrain-based assisted navigation method when executed by a processor.

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

[0040] This invention introduces constrained fully symmetric polytopes from set membership filtering into a seabed terrain-assisted navigation method, reducing the algorithm's operational overhead while also addressing the unknown distribution of measurement noise from low-resolution prior nautical charts. The method extracts appropriate water depth data from the prior nautical chart using depth contours and grid points, and outsources this data using a constrained fully symmetric polytope. This allows the real water depth data to be contained within the constrained fully symmetric polytope, improving the robustness of the navigation method without increasing system computational overhead. The effectiveness of navigation results is evaluated by analyzing the water depth difference and volume of the geometric center of the constrained fully symmetric polytope, enhancing the feasibility of the navigation method in practical applications. BRIEF DESCRIPTION OF THE DRAWINGS

[0041] Figure 1 It is a structural diagram of the present invention.

[0042] Figure 2 It is a flow chart of the present invention.

[0043] Figure 3 It is a flow chart of navigation result validity evaluation in the present invention.

[0044] Figure 4 It is a schematic diagram of the process of reducing the complexity and dimension of constrained fully symmetric polytopes in the present invention. DETAILED DESCRIPTION

[0045] The present invention will be further described below with reference to the accompanying drawings.

[0046] In order to solve the problems of the current single-beam sonar submarine terrain assisted navigation method, which has high computational consumption and difficulty in processing unknown measurement noise distribution due to the particle filtering method, the present invention provides a submarine terrain assisted navigation method.

[0047] At the same time, in order to address the problem that in actual applications, the validity of navigation results cannot be known, which limits the application of terrain-assisted navigation methods, the present invention provides a method for evaluating the validity of navigation results. This method applies set membership filtering theory to the submarine terrain-assisted navigation method. On the basis of ensuring the robustness of the terrain-assisted navigation method, it effectively reduces the computational consumption of the navigation method, realizes efficient and robust submarine terrain-assisted navigation, and completes the evaluation of the validity of navigation results by studying the set obtained by set membership filtering.

[0048] The specific process of the present invention is as follows:

[0049] Step 1: Using the inertial navigation system on board the underwater vehicle as a reference, at the initial moment, obtain the horizontal position coordinates (x * ,y * ) and error range; (x * ,y *is the center of the constrained zonotope, and is the generating vector of the constrained zonotope

[0050] wherein, is the center of the constrained zonotope, is the generating vector of the constrained zonotope, and is the constraint condition;

[0051] Step 2: According to the inertial navigation system, the motion vector of the underwater vehicle from the last time to the current time is obtained, and the center position and the generating vector of the last time constrained zonotope are updated to obtain the motion updated constrained zonotope Step 3: According to the single-beam sonar and depth gauge carried by the underwater vehicle, the water depth measurement value of the position of the underwater vehicle at the current time is obtained.

[0052]

[0053] wherein, u t-1 represents the motion vector of the underwater vehicle from the last time to the current time provided by the inertial navigation system; represents the constrained zonotope corresponding to the motion noise;

[0054] Step 3: According to the single-beam sonar and depth gauge carried by the underwater vehicle, the water depth measurement value of the position of the underwater vehicle at the current time is obtained.

[0055] Step 4: In the prior map, a search radius is set to determine a search range with (x * ,y * ) as the center, so as to ensure that the real position of the underwater vehicle is within the search range.

[0056] The terrain undulation of each region in the prior map is calculated in advance, and the numerical values of the terrain undulations of all regions are normalized to the range of 0.5-1.

[0057] The calculation method of the terrain undulation is:

[0058]

[0059] wherein, M and N are the length and width of the calculation region respectively; h(i,j) represents the water depth data at the (i,j) position in the prior map.

[0060] According to the error range of the inertial navigation system at the current time obtained in step 1, the initial value R0 of the search radius is determined, and the normalized value of the terrain undulation of the region where (x * ,y * ) is located is The search radius R is set as:

[0061] Step 5: In the search range of the prior chart, considering the error of the water depth measurement, all the depth lines within the error range are extracted, and the extracted depth lines are discretized into depth line points; using grid search, the center points of the grids within the error range are extracted;

[0062] Step 6: The extracted depth line points and grid center points are constrained and fully symmetrical polyhedron wrapped, ensuring that all depth line points and grid center points are fully included, and the corresponding constrained full symmetrical polyhedron with the smallest volume is constructed

[0063] Step 7: Calculate The intersection of The geometric position center coordinates of are taken as the horizontal position coordinate estimate of the underwater vehicle at the current time;

[0064]

[0065] Where R is a matrix, satisfying

[0066] The center of the symmetrical polyhedron is given as However, the constrained full symmetrical polyhedron has a constraint condition, which causes the geometric center at this time to no longer be the center in the formula Therefore, the combined center coordinates need to be calculated separately according to the vertex coordinates. An example of a full symmetrical polyhedron and a constrained full symmetrical polyhedron is given, where the center is c Z = [1; 1], the generation matrix is G Z = [0 2 0 1; 0 0 2 1], the constraint condition A Z = [-1 -1-1 1], b Z = [2]. At this time, the geometric center of the constrained full symmetrical polyhedron is c g = [2 / 3, 2 / 3], so the geometric center of the constrained full symmetrical polyhedron needs to be calculated as the estimated position.

[0067] However, as the motion update progresses, the order and dimension of the constrained full symmetrical polyhedron also gradually increase, causing a large amount of computational consumption, so the constrained full symmetrical polyhedron needs to be executed for dimension and complexity reduction, reducing algorithmic computational consumption;

[0068] Dimension reduction is divided into two steps, first, the constraints are eliminated until the constraint n c reaches the specified index, and in the process of eliminating n c , an n g ​, so the complexity order is o d remains unchanged, and then the generator n g Perform dimensionality reduction to convert o d Reduce to within the specified indicator range.

[0069] Calculate the system order and system dimension of the constrained fully symmetric polytope at this time. If the system complexity is greater than the set system order threshold and dimension thresholds Rescaling is used to transfer information from the constraints to the generator. Without changing the fully symmetric polytope of the constraints, the unit hypercube is changed and linearly changed to achieve the scaling of the unit hypercube, thereby reducing the constraint information. The complexity order calculation formula is as follows:

[0070] o d =(n g -n c ) / n

[0071] Among them, n g is the dimension of the generator, n c is the dimension of the constraint, and n is the dimension of the constrained fully symmetric polytope.

[0072] After rescaling, dimensionality reduction is performed on the high-dimensional constrained fully symmetric polytope, and the generated matrix is ​​reduced in dimensionality until it meets the threshold requirement;

[0073] Step 8: Obtain the water depth value at the current moment of the estimated horizontal position coordinate value of the underwater vehicle in the prior chart, calculate the error between the water depth value and the water depth measurement value; and compare the error with the Z xt The volume of is input into the validity evaluation function to obtain the validity evaluation result of the navigation result; if the navigation result at the current moment is determined to be valid, the horizontal position coordinate estimate of the underwater vehicle at the current moment is fed back to the inertial navigation system for correction;

[0074] Single-beam sonar provides limited information in a single measurement, making it impossible to evaluate the effectiveness of navigation results. This results in a lack of understanding of the validity of the output position information, even though the terrain-assisted navigation output is obtained. This invention uses a twin support vector machine to evaluate the effectiveness of navigation results, completing the effectiveness evaluation of each matching result and improving the effectiveness of terrain-assisted navigation methods in practical applications.

[0075] A training data set is constructed, and the difference between the water depth values ​​and the volume of the constrained fully symmetric polyhedron is used as the input of the twin support vector machine, and the navigation error is used as the output of the twin support vector machine. After training, the regression relationship between the difference between the volume of the constrained fully symmetric polyhedron and the water depth values ​​and the validity of the navigation results is obtained. The regression function is the validity evaluation function.

[0076] The volume of a constrained fully symmetric polytope is calculated by treating the constrained fully symmetric polytope as a convex polyhedron. The vertex coordinates of the convex polyhedron are obtained based on the constrained fully symmetric polytope. The convex polyhedron is then divided into multiple triangles using Delaunay triangulation. After obtaining the multiple triangles, the area of ​​each triangle is calculated to obtain the area of ​​the convex polyhedron, which is the volume of the constrained fully symmetric polytope.

[0077] Step 9: If the underwater vehicle has not completed the mission, return to step 2 and continue navigation.

[0078] Combine Figure 1 The overall implementation method is explained. First, water depth data is obtained through single-beam sonar and depth meter. The inertial navigation system is used to provide attitude and reference trajectory. According to the reference position provided by the inertial navigation, a suitable search area is selected in the prior nautical chart. The appropriate water depth data points are extracted through contour line / grid point extraction and used to constrain the fully symmetric polyhedral terrain to assist navigation. The matched and corrected position is then fed back to the inertial navigation system to correct the accumulated error of the inertial navigation system.

[0079] Combine Figure 2 The overall algorithm flow chart is described. The present invention first completes the initialization and motion update of the constrained fully symmetric polytope through the initial conditions and motion update information provided by the inertial navigation system, and uses the water depth information obtained by measuring the single-beam sonar and the depth meter to extract the isobath points / grid points that meet the measurement error range within the search range, and outsources the water depth points through the constrained fully symmetric polytope. The intersection of the observed constrained fully symmetric polytope and the constrained fully symmetric polytope after the motion update is calculated, and is outsourced using the new constrained fully symmetric polytope. The order and complexity of the constrained fully symmetric polytope at this time are calculated, and it is determined whether the order and complexity reduction are executed. The geometric position center of the constrained fully symmetric polytope is calculated, the matching position is estimated, the effectiveness of the navigation result is evaluated, and it is determined whether the end point is reached.

[0080] Combine Figure 3 This section describes how to implement navigation result validity assessment. Based on the motion-updated constrained fully symmetric polytope, the volume of the constrained fully symmetric polytope and the water depth difference at its geometric center are calculated. The volume and water depth difference are used as inputs to a twin support vector machine (SVM), and the navigation error is used as the SVM output. Data regression is performed to obtain a regression relationship between the constrained fully symmetric polytope parameters and the navigation result, completing the navigation result validity assessment.

[0081] Combine Figure 4The implementation of the constrained zonotope complexity and dimension reduction is illustrated.The order of the constrained zonotope after observation update is calculated, and it is judged whether the order is greater than the set threshold value.If the order is greater than the set threshold value, rescaling is performed, and then it is judged whether the dimension of the constrained zonotope is greater than the set threshold value.If the dimension is greater than the set threshold value, system dimension reduction is performed, and a new constrained zonotope is generated.

[0082] In the seabed terrain aided navigation method, a single-beam sonar is used as a measurement sensor to reduce system energy consumption, but the resulting lack of measurement information leads to poor robustness of the navigation result.

[0083] The constrained zonotope in the set member filtering is introduced into the seabed terrain aided navigation method, the running consumption of the algorithm is reduced, and the measurement noise of the low-resolution prior sea chart unknown distribution can be processed, in the constrained zonotope seabed terrain aided navigation method, the appropriate water depth data are extracted from the prior sea chart by using the contour lines and grid points, and the water depth data are enclosed by the constrained zonotope, so that the real water depth data are in the constrained zonotope, the robustness of the navigation method is improved without increasing the system calculation consumption, and the order and dimension of the system are calculated, the complexity of the system is judged, and the complexity and dimension reduction is performed, so that the running efficiency of the algorithm is further improved.

[0084] The present application fully considers the problems existing in the seabed terrain aided navigation method with a single-beam sonar as a measurement sensor and a low-resolution prior sea chart, and proposes a corresponding solution, which improves the robustness of the navigation result and completes the effectiveness evaluation of the navigation result on the basis of maintaining low calculation consumption.

[0085] The above only describes the preferred embodiments of the present application and is not used to limit the present application, and the present application can have various changes and variations for those skilled in the art.Any modification, equivalent replacement, improvement, etc., made within the spirit and principle of the present application shall be included in the protection scope of the present application.

Claims

1. A method for assisting navigation based on seabed topography, characterized in that: The following steps are involved: Step 1: At the initial moment, obtain the horizontal position coordinates (x * ,y * ) and error range; (x * ,y * ) as the center, and under the premise of ensuring that the horizontal position coordinates of the underwater vehicle within the error range are all included, a constrained fully symmetric polytope with the smallest corresponding volume is constructed. Step 2: According to the inertial navigation system, obtain the motion vector of the underwater vehicle from the previous moment to the current moment, and update the constrained fully symmetric polytope at the previous moment The center position and generating vector of the motion updated constrained fully symmetric polytope are obtained. Step 3: Obtain the water depth measurement value of the underwater vehicle's current location based on the single-beam sonar and depth meter carried by the underwater vehicle; Step 4: In the prior chart, set the search radius to (x * ,y * ) as the center to determine the search range and ensure that the actual position of the underwater vehicle is within the search range; Step 5: Within the search range of the prior chart, taking into account the error of the water depth measurement value, all the isobaths with water depths within the error range are extracted, and the extracted isobaths are discretized into isobath points; grid search is used to extract the center point of the grid with water depths within the error range; Step 6: Outsource the extracted depth contour points and grid center points to constrained fully symmetrical polytopes, and construct a constrained fully symmetrical polytope with the minimum corresponding volume while ensuring that all depth contour points and grid center points are fully included. Step 7: Calculation and intersection of Will The geometric position center coordinates are used as the estimated horizontal position coordinates of the underwater vehicle at the current moment; Step 8: Obtain the water depth value at the current moment of the estimated horizontal position coordinate value of the underwater vehicle in the priori nautical chart, calculate the error between the water depth value and the water depth measurement value; and compare the error with the water depth value. The volume of is input into the validity evaluation function to obtain the validity evaluation result of the navigation result; if the navigation result at the current moment is determined to be valid, the horizontal position coordinate estimate of the underwater vehicle at the current moment is fed back to the inertial navigation system for correction; Step 9: If the underwater vehicle has not completed the mission, return to step 2 and continue navigation.

2. The method for assisting navigation based on seabed topography according to claim 1, characterized in that: The step 2 is specifically as follows: Among them, u t-1 Represents the motion vector of the underwater vehicle from the previous moment to the current moment provided by the inertial navigation system; Represents the constrained fully symmetric polytope corresponding to the motion noise.

3. The method for assisting navigation based on seabed topography according to claim 1, characterized in that: The method for setting the search radius in step 4 is: Pre-calculate the terrain relief of each area in the prior nautical chart, and normalize the values ​​of the terrain relief of all areas to the range of 0.5-1; According to the error range of the inertial navigation system at the current moment obtained in step 1, the initial value R0 of the search radius is determined. According to (x * ,y * ) is the normalized value of the terrain relief in the area The search radius R is set to:

4. The method for assisting navigation based on seabed topography according to claim 3, characterized in that: The calculation method of the terrain relief is: Where M and N are the length and width of the calculation area respectively; h(i,j) represents the water depth data at position (i,j) in the prior nautical chart.

5. The method for assisting navigation based on seabed topography according to claim 1, characterized in that: The calculation in step 7 and intersection of Specifically: The constrained fully symmetric polytope is constructed as follows: Z=<c,G,A,b> Where c represents the center of the constrained fully symmetric polytope Z, G is the generating vector, and A and b are the constraints; and intersection of The calculation method is: Where R is a matrix that satisfies 6. The method for assisting navigation based on seabed topography according to claim 1, characterized in that: The method for obtaining the effectiveness evaluation function in step 8 is: A training data set is constructed, and the difference between the water depth values ​​and the volume of the constrained fully symmetric polyhedron is used as the input of the twin support vector machine, and the navigation error is used as the output of the twin support vector machine. After training, the regression relationship between the difference between the volume of the constrained fully symmetric polyhedron and the water depth values ​​and the validity of the navigation results is obtained. The regression function is the validity evaluation function.

7. The method for assisting navigation based on seabed topography according to claim 1, characterized in that: In step 7, the constrained fully symmetric polytope Z is obtained. xt After that, the dimension and complexity are reduced to reduce the computational cost of the algorithm.

8. A computer device / apparatus / 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 7.

9. 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 7 are implemented.

10. 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 7 are implemented.

Images