A terrain navigation method, device, equipment and storage medium for underwater vehicle

Abstract

The present disclosure provides a terrain navigation method, device, equipment and storage medium for an underwater vehicle, including: an inertial navigation system calculates first position and attitude data of the vehicle based on attitude, speed information and initial position; calculates second position and attitude data at the next moment based on the water depth data and attitude and speed information; calculates motion update information based on the first position and attitude data and the second position and attitude data to establish a variable-scale priori seabed topography map; establishes an initial fully symmetric polytope based on the initial position and a predicted error range, updates the initial fully symmetric polytope based on the motion update information and the variable-scale priori seabed topography map to obtain a measured updated fully symmetric polytope; calculates an estimated position of the vehicle based on the measured updated fully symmetric polytope, and corrects the accumulated error of the inertial navigation system based on the estimated position.

Description

Technical Field

[0001] The present disclosure relates to the field of underwater navigation and positioning technology, and in particular to a terrain navigation method, device, equipment and storage medium for an underwater vehicle. Background Art

[0002] With the development of underwater vehicle battery technology, the endurance of underwater vehicles is gradually improving. The seabed terrain navigation method is an important means to achieve robust and accurate navigation in underwater vehicle long-term underwater missions. Priori seabed topographic maps and real-time terrain measurement sensors are important components of the terrain navigation system. In terms of priori seabed topographic maps, as more and more surveying and mapping methods and technologies are used for underwater terrain mapping, there are many public seabed topographic maps that provide low-resolution water depth data worldwide through gravity inversion and encryption of a small number of measured points. In terms of real-time seabed terrain measurement sensors, single-beam bathymetric sonar has lower energy consumption than multi-beam bathymetric sonar, which is currently widely used for terrain navigation, and can extend the endurance of underwater vehicles to a limited extent. Therefore, for underwater vehicles that need to conduct long-term underwater navigation in global waters, relying on publicly downloadable global seabed topographic maps and single-beam bathymetric sonar for terrain navigation is an important means to achieve long-term and accurate underwater navigation of underwater vehicles.

[0003] Existing methods use the seabed contour map as a prior seabed topographic map, and change the particle weight update method to achieve robust navigation results on low-resolution prior seabed topographic maps with poor terrain features. However, this method does not take into account the inaccuracy of the prior terrain data of the global seabed topographic map, and the possible perceptual ambiguity of the seabed topography, which makes it difficult to achieve long-term accurate underwater navigation based on public seabed topographic maps. Or divide the terrain navigation system to achieve more stable positioning information and more efficient filtering convergence speed when there are multiple pseudo-peaks in the positioning likelihood function. However, this method does not design a terrain navigation system solution, and does not consider the low resolution and low confidence problems of using public seabed topographic maps, making it unsuitable for underwater navigation using public seabed topographic maps. Summary of the invention

[0004] The present disclosure provides a seabed terrain navigation method, device, equipment and storage medium for an underwater vehicle to at least solve the above technical problems existing in the prior art.

[0005] According to a first aspect of the present disclosure, there is provided a seabed terrain navigation method for an underwater vehicle, comprising:

[0006] Acquiring the initial position, attitude and speed information of the aircraft, and the inertial navigation system calculating the first position attitude data of the aircraft based on the attitude, speed information and the initial position;

[0007] Detecting water depth data of the aircraft at the next moment, wherein the inertial navigation system calculates second position and attitude data at the next moment based on the water depth data and the attitude and speed information;

[0008] Calculate motion update information based on the first position and posture data and the second position and posture data;

[0009] Acquire a rough seafloor topographic map and a local fine seafloor topographic map respectively, match the local fine seafloor topographic map with the corresponding position in the rough seafloor topographic map, and establish a variable-scale prior seafloor topographic map;

[0010] An initial fully symmetric polytope is established based on the initial position and the predicted error range, and the initial fully symmetric polytope is updated based on the motion update information and the variable scale prior seafloor topography map to obtain a measured updated fully symmetric polytope;

[0011] The fully symmetric polytope is updated based on the measurements to calculate an estimated position of the vehicle, and a cumulative error of the inertial navigation system is corrected based on the estimated position.

[0012] In one possible implementation manner, detecting the water depth data of the aircraft at the next moment includes:

[0013] Acquire the bottom altitude, sound speed information and depth information of the aircraft based on the detection device, and correct the bottom altitude based on the first position attitude data and the sound speed information;

[0014] The water depth data at the next moment is calculated based on the depth information and the corrected bottom height.

[0015] In one possible implementation manner, performing error correction on the inertial navigation system based on the motion update information includes:

[0016] A priori topographic map of the area where the aircraft is located is obtained, the priori topographic map is motion updated based on the motion update information, and the inertial navigation system is error corrected based on the priori topographic map after motion update and the water depth data.

[0017] In one possible implementation, the updating of the initial fully symmetric polytope based on the motion update information and the variable-scale priori seafloor topography map includes:

[0018] detecting motion noise of the aircraft, and establishing a noise fully symmetric polytope based on the motion noise;

[0019] Performing motion updating on the initial fully symmetric polytope based on the motion update information and the noise fully symmetric polytope to obtain a motion updated fully symmetric polytope;

[0020] Acquire at least one water depth point area where the aircraft is located, and establish a fully symmetric polytope of observation information for each of the water depth point areas;

[0021] The intersection of the observation information fully symmetric polytope and the motion update fully symmetric polytope is calculated, and the measurement update fully symmetric polytope is established based on the intersection.

[0022] In one possible implementation manner, the obtaining of at least one water depth point area of ​​the location of the aircraft includes:

[0023] Dividing the variable-scale priori seafloor topography map into grids to obtain at least one sub-map;

[0024] Obtaining a minimum water depth value, a maximum water depth value, and an area side length of each area corresponding to the sub-map, and calculating a slope value corresponding to each sub-map based on the minimum water depth value, the maximum water depth value, and the area side length;

[0025] The inertial navigation system sets a search interval based on the water depth data and the slope value of the current position of the vehicle, and extracts a water depth point data set within the search interval based on the variable-scale priori seafloor topographic map;

[0026] The water depth point data set is segmented based on Euclidean clustering to obtain at least one water depth point area.

[0027] In one possible implementation, the updating of the fully symmetric polytope based on the measurement calculates the estimated position of the vehicle, and corrects the accumulated error of the inertial navigation system based on the estimated position, including:

[0028] Obtaining the measured water depth value at the location of the aircraft, and calculating the water depth value at the center position of the measured updated fully symmetric polytope and the volume of the measured updated fully symmetric polytope;

[0029] Calculate the weight of the measured updated fully symmetric polytope based on the measured water depth value, the center position of the measured updated fully symmetric polytope and the volume and the center position water depth value, and calculate the estimated position of the aircraft based on the weight and the center position of the measured updated fully symmetric polytope;

[0030] The accumulated error of the inertial navigation system is corrected based on the estimated position.

[0031] In one embodiment, the method further comprises:

[0032] monitoring the number of the measured updated fully symmetric polytopes in real time, and when the number exceeds a first threshold, obtaining the center position of each of the measured updated fully symmetric polytopes;

[0033] Based on the center position, calculate the center distance between every two of the measured updated fully symmetric polytopes, sort the calculation results by size, and obtain a first position data set;

[0034] Merging the two measured updated fully symmetric polytopes corresponding to the smallest center distance in the first position data set, and recalculating the center distance between every two measured updated fully symmetric polytopes to update the first position data set;

[0035] When the number of the measurement update fully symmetric polytopes corresponding to the updated first position data set is less than the first threshold, stopping the merging of the measurement update fully symmetric polytopes;

[0036] The complexity of each obtained fully symmetric polytope is calculated, and when the complexity exceeds a second threshold, a complexity reduction operation is performed on the fully symmetric polytope to reduce the algorithm calculation consumption.

[0037] According to a second aspect of the present disclosure, there is provided a seabed terrain navigation device for an underwater vehicle, the device comprising:

[0038] An information acquisition unit, used to acquire the initial position, attitude and speed information of the aircraft, the inertial navigation system calculates the first position attitude data of the aircraft based on the attitude, speed information and the initial position, detects the water depth data of the aircraft at the next moment, and the inertial navigation system calculates the second position attitude data at the next moment based on the water depth data and the attitude and speed information;

[0039] A motion update information acquisition unit, configured to calculate motion update information based on the first position and posture data and the second position and posture data;

[0040] A seabed topographic map establishing unit is used to respectively obtain a rough seabed topographic map and a local fine seabed topographic map, match the local fine seabed topographic map with the corresponding position in the rough seabed topographic map, and establish a variable-scale priori seabed topographic map;

[0041] A fully symmetric polytope updating unit is used to establish an initial fully symmetric polytope based on the initial position and the predicted error range, and to update the initial fully symmetric polytope based on the motion update information and the variable scale prior seafloor topography map to obtain a measured updated fully symmetric polytope;

[0042] An error correction unit is used to calculate the estimated position of the spacecraft based on the measurement update full symmetric polytope, and to correct the accumulated error of the inertial navigation system based on the estimated position.

[0043] According to a third aspect of the present disclosure, there is provided an electronic device, including:

[0044] at least one processor; and

[0045] a memory communicatively connected to the at least one processor; wherein,

[0046] The memory stores instructions that can be executed by the at least one processor, and the instructions are executed by the at least one processor to enable the at least one processor to perform the method described in the present disclosure.

[0047] According to a fourth aspect of the present disclosure, a non-transitory computer-readable storage medium storing computer instructions is provided, wherein the computer instructions are used to cause the computer to execute the method described in the present disclosure.

[0048] The present invention discloses a method, device, equipment and storage medium for seabed terrain navigation of an underwater vehicle. A variable-scale priori seabed terrain map is established based on a rough seabed terrain map and a local fine seabed terrain map. Fully symmetric polytopes are then applied to the terrain navigation method to estimate unknown distributions. The estimation of situations where there are perception ambiguities is achieved through fully symmetric polytopes, which effectively improves the accuracy of navigation results. The computational consumption of the algorithm is further reduced by performing dimensionality reduction and merging with fully symmetric polytopes, ultimately achieving long-term, robust single-beam bathymetric sonar terrain navigation based on public seabed terrain maps.

[0049] It should be understood that the content described in this section is not intended to identify the key or important features of the embodiments of the present disclosure, nor is it intended to limit the scope of the present disclosure. Other features of the present disclosure will become easily understood through the following description. BRIEF DESCRIPTION OF THE DRAWINGS

[0050] The above and other objects, features and advantages of the exemplary embodiments of the present disclosure will become readily understood by reading the detailed description below with reference to the accompanying drawings. In the accompanying drawings, several embodiments of the present disclosure are shown in an exemplary and non-limiting manner, in which:

[0051] In the drawings, the same or corresponding reference numerals represent the same or corresponding parts.

[0052] Figure 1 The schematic diagram shows the implementation process of a method for seabed terrain navigation of an underwater vehicle according to an embodiment of the present disclosure. Figure 1 ;

[0053] Figure 2 The schematic diagram shows the implementation process of a method for seabed terrain navigation of an underwater vehicle according to an embodiment of the present disclosure. Figure 2 ;

[0054] Figure 3An algorithm flow chart based on a fully symmetric polytope according to an embodiment of the present disclosure is shown;

[0055] Figure 4 A schematic diagram of a seabed terrain navigation device for an underwater vehicle according to an embodiment of the present disclosure is shown;

[0056] Figure 5 A schematic diagram of the structure of an electronic device according to an embodiment of the present disclosure is shown. DETAILED DESCRIPTION

[0057] In order to make the purpose, features, and advantages of the present disclosure more obvious and easy to understand, the technical solutions in the embodiments of the present disclosure will be clearly and completely described below in conjunction with the drawings in the embodiments of the present disclosure. Obviously, the described embodiments are only part of the embodiments of the present disclosure, not all of the embodiments. Based on the embodiments in the present disclosure, all other embodiments obtained by those skilled in the art without creative work are within the scope of protection of the present disclosure.

[0058] Figure 1 The schematic diagram shows the implementation process of a method for seabed terrain navigation of an underwater vehicle according to an embodiment of the present disclosure. Figure 1 ,like Figure 1 As shown, the seabed terrain navigation process of an underwater vehicle in an embodiment of the present disclosure includes the following steps:

[0059] Step 101, obtaining the initial position, attitude and speed information of the aircraft, and the inertial navigation system calculating the first position and attitude data of the aircraft based on the attitude, speed information and the initial position; detecting the water depth data of the aircraft at the next moment, and the inertial navigation system calculating the second position and attitude data at the next moment based on the water depth data and the attitude and speed information.

[0060] In the disclosed embodiment, the aircraft obtains the initial position, attitude, and speed information of the aircraft, wherein the initial position is the initial position of the aircraft when it dives to the specified underwater operating depth, and uses an inertial navigation system in conjunction with an acoustic Doppler velocimeter to calculate the attitude information of this position based on the initial position, attitude, and speed information, i.e., first position attitude data. The bottom height of the aircraft is obtained based on a single-beam bathymetric sonar, the speed of sound information is obtained based on a sonic velocity meter, and the depth information is obtained based on a depth meter. The terrain navigation computer processes the collected data, and corrects the bottom height collected by the single-beam bathymetric sonar based on the first position attitude data and the speed of sound information; and calculates the water depth data at the next moment based on the depth information and the corrected bottom height. The inertial navigation system calculates the second position attitude data at the next moment based on the water depth data and the attitude and speed information.

[0061] Step 102: Calculate motion update information based on the first position and posture data and the second position and posture data.

[0062] In the disclosed embodiment, the motion update information is calculated based on the position of the inertial navigation system at the previous and next moments, i.e., the first position attitude data and the second position attitude data. The priori topographic map of the area where the vehicle is located is obtained through a global seabed topography storage computer, the priori topographic map is motion updated based on the motion update information, and the inertial navigation system is error corrected based on the priori topographic map after motion update and the water depth data.

[0063] Step 103, respectively obtain a rough seafloor topography map and a local fine seafloor topography map, match the local fine seafloor topography map with the corresponding position in the rough seafloor topography map, and establish a variable-scale priori seafloor topography map.

[0064] In the disclosed embodiment, a publicly downloadable global low-resolution seabed topographic map, i.e., a rough seabed topographic map, and a publicly downloadable small-scale local area high-resolution seabed topographic map, i.e., a local fine seabed topographic map, are used. The local fine seabed topographic map is matched with the corresponding positions in the rough seabed topographic map to establish a variable-scale prior seabed topographic map.

[0065] Step 104, establishing an initial fully symmetric polytope based on the initial position and the predicted error range, and updating the initial fully symmetric polytope based on the motion update information and the variable scale prior seabed topography map to obtain a measured updated fully symmetric polytope.

[0066] In the disclosed embodiment, the initial position is used as the center position of the initial fully symmetric polytope, and the predicted error range is used to establish the matrix of the initial fully symmetric polytope, so that the actual position of the aircraft is in the generated initial fully symmetric polytope.

[0067] In an embodiment of the present disclosure, the motion noise of the aircraft is detected, and a noise fully symmetric polytope is established based on the motion noise; the initial fully symmetric polytope is motion updated based on the motion update information and the noise fully symmetric polytope to obtain a motion updated fully symmetric polytope.

[0068] In the disclosed embodiment, at least one water depth point area of ​​the position of the aircraft is obtained, and an observation information fully symmetric polytope is established for each of the water depth point areas. The variable scale prior seafloor topographic map is divided into grids to obtain at least one sub-map; the minimum water depth value, the maximum water depth value and the area side length of each sub-map corresponding area are obtained, and the slope value corresponding to each sub-map is calculated based on the minimum water depth value, the maximum water depth value and the area side length; the inertial navigation system sets a search interval based on the water depth data and the slope value of the current position of the aircraft, wherein the size of the search interval is related to the terrain slope, a small search interval is set in a place with a large terrain slope, and a large search interval is set in a place with a small terrain slope, so as to realize adaptive adjustment of the search area based on the terrain slope. The water depth point data set within the search interval is extracted based on the variable scale prior seafloor topographic map; the water depth point data set is segmented based on Euclidean clustering to obtain at least one water depth point area. The intersection of the observation information fully symmetric polytope and the motion update fully symmetric polytope is calculated, and the measurement update fully symmetric polytope is established based on the intersection.

[0069] Step 105: Calculate the estimated position of the vehicle based on the measurement-updated fully symmetric polytope, and correct the accumulated error of the inertial navigation system based on the estimated position.

[0070] In the disclosed embodiment, the measured water depth value of the position of the aircraft is obtained, and the water depth value of the center position of the measured updated fully symmetric polytope and the volume of the measured updated fully symmetric polytope are calculated;

[0071] Since directly using volume to represent the weight of a fully symmetrical polytope will result in a large area of ​​water depth, and the fully symmetrical polytope that is not inside the fully symmetrical polytope will occupy a large amount of weight, resulting in errors in the calculation results. Therefore, the weight of each fully symmetrical polytope is obtained based on the measured water depth value, the water depth value at the center position of the fully symmetrical polytope and the measured updated volume of the fully symmetrical polytope, and the estimated position of the vehicle is calculated based on the weight and the measured updated center position of the fully symmetrical polytope; the accumulated error of the inertial navigation system is corrected based on the estimated position, and then the motion update and measurement update are executed cyclically until the end point is reached.

[0072] In the disclosed embodiment, in order to reduce the number and complexity of fully symmetric polytopes, the number of fully symmetric polytopes updated by measurement is monitored in real time. When the number exceeds a first threshold, wherein the first threshold can be set based on experience, the center position of each fully symmetric polytope updated by measurement is obtained; based on the center position, the center distance between every two fully symmetric polytopes updated by measurement is calculated, and the calculation results are sorted by size to obtain a first position data set; the two fully symmetric polytopes updated by measurement corresponding to the smallest center distance in the first position data set are merged, and the center distance between every two fully symmetric polytopes updated by measurement is recalculated to update the first position data set; when the number of fully symmetric polytopes updated by measurement corresponding to the updated first position data set is less than the first threshold, the merging of the fully symmetric polytopes updated by measurement is stopped; complexity calculation is performed on each obtained fully symmetric polytope, and when the complexity exceeds a second threshold, complexity reduction operation is performed on the fully symmetric polytope to reduce algorithm calculation consumption.

[0073] Figure 2 The schematic diagram shows the implementation process of a method for seabed terrain navigation of an underwater vehicle according to an embodiment of the present disclosure. Figure 2 ,like Figure 2 As shown, a seabed terrain navigation method for an underwater vehicle in an embodiment of the present disclosure includes the following steps:

[0074] Step 201, collecting measurement data of monitoring equipment in the aircraft.

[0075] In the disclosed embodiment, before the aircraft performs underwater operations, the Global Navigation Satellite System (GNSS) is used to complete a long-term alignment of the inertial navigation system to improve the attitude accuracy and navigation accuracy of the inertial navigation system during underwater navigation. The aircraft uses the electric energy stored in the battery compartment to dive and complete specific underwater tasks through the cooperation of the thruster and the rudder. During the dive, the aircraft is positioned through an ultra-short baseline to prevent the positioning accuracy of the inertial navigation system from decreasing due to diving too deep, and starts underwater operations after reaching the specified depth.

[0076] In the disclosed embodiment, the monitoring equipment includes: an acoustic Doppler velocimeter, a single-beam depth sounding sonar, a sound velocity meter, and a depth meter. The inertial navigation system carried by the aircraft cooperates with the acoustic Doppler velocimeter to provide reference position and attitude information. A terrain navigation computer is provided in the control cabin of the aircraft. The attitude and position information provided by the inertial navigation system at this time is collected, and the bottom height of the aircraft is obtained by using the single-beam depth sounding sonar, the sound velocity information provided by the sound velocity meter, and the depth information provided by the depth meter are collected.

[0077] Step 202: The terrain navigation computer processes the collected measurement data to correct the error of the inertial navigation system.

[0078] In the disclosed embodiment, the heading angle, roll angle and pitch angle provided by the inertial navigation system assisted by the acoustic Doppler velocimeter and the coordinates of the underwater vehicle in the east-north-earth coordinate system are used to convert the water depth data in the carrier coordinate system into the geodetic coordinate system and send it to the terrain navigation computer.

[0079] In the disclosed embodiment, the terrain navigation computer corrects the data of the single-beam bathymetric sonar through the sound speed information and the attitude information, obtains the water depth data in combination with the depth information, obtains the motion update information according to the position calculation of the inertial navigation system at the previous and next moments, obtains the corresponding regional prior topographic map sent by the global seabed terrain storage computer, completes the motion update using the motion update information provided by the inertial navigation system, performs terrain navigation according to the real-time water depth data and the corresponding regional prior topographic map sent by the global seabed terrain storage computer, matches the measured water depth data of the underwater vehicle with the regional prior topographic map in the terrain navigation computer, obtains the underwater vehicle position estimation result, and feeds back the corresponding position to the inertial navigation system to realize the correction of the inertial navigation system error.

[0080] Step 203, establishing a variable-scale seafloor topographic map system.

[0081] In the disclosed embodiment, in the terrain navigation system, known prior seafloor topographic map data is first processed to obtain a large-area public rough seafloor topographic map and a small-area high-precision prior seafloor topographic map. By considering the different navigation performances of the two prior seafloor topographic maps in terrain navigation, a variable-scale prior seafloor topographic map system is established to improve the navigation performance of the terrain navigation method, and to achieve a rough matching of the rough seafloor topographic map based on the prior seafloor topographic map area and a precise matching of the accurate seafloor topographic map. Among them, the area public rough seafloor topographic map and the small-area high-precision prior seafloor topographic map can be publicly downloaded and obtained.

[0082] In the embodiment of the present disclosure, the prior seabed topography map is divided into regional grids according to the grid division standard to obtain M sub-maps of the prior seabed topography map, and then the terrain slope in each sub-map area of ​​the prior seabed topography map is obtained by the terrain slope calculation formula, and the calculation formula is as follows:

[0083]

[0084] in, Indicates the maximum water depth in the sub-map area. Indicates the minimum water depth in the sub-map area. Indicates the side length of the submap area.

[0085] Step 204, establish a set membership filter framework for seabed terrain navigation, and use fully symmetric polytopes to improve terrain navigation accuracy.

[0086] In the disclosed embodiments, since the rough prior seabed topographic map causes the distribution of terrain data measurement errors to be unknown and difficult to count, fully symmetric polytopes are used to estimate the true state to improve the accuracy of underwater vehicle position estimation. At the same time, in view of the situation where there is perceptual ambiguity in the seabed terrain, a terrain navigation method based on multiple fully symmetric polytopes is proposed to reduce the algorithm calculation consumption while ensuring navigation performance.

[0087] Figure 3 FIG. 4 shows an algorithm flow chart based on a fully symmetric polytope according to an embodiment of the present disclosure, such as Figure 3 As shown, an algorithm based on a fully symmetric polytope in an embodiment of the present disclosure includes the following steps:

[0088] Step 301, initializing the fully symmetric polytope.

[0089] In the disclosed embodiment, the fully symmetric polytope is initialized according to the initial position and initial error range provided by the inertial navigation system, and the initial fully symmetric polytope is obtained as shown below:

[0090]

[0091] Among them, represents the center of the fully symmetric polytope, The center of the initial fully symmetrical polytope is determined according to the initial position provided by the inertial navigation system, and a suitable generating matrix needs to be designed according to the predicted initial error of the inertial navigation system so that the actual position of the underwater vehicle 1 is in the generated fully symmetrical polytope.

[0092] Step 302, observe and update the fully symmetric polytope.

[0093] In the embodiment of the present disclosure, a noise fully symmetric polytope is established based on the motion noise, and the initialized fully symmetric polytope is updated according to the motion update information provided by the inertial navigation system, as shown below:

[0094]

[0095] in represents the fully symmetric polytope at time k-1, Indicates sports update information. Represents the noise fully symmetric polytope corresponding to the motion noise.

[0096] In the disclosed embodiment, water depth measurement data is obtained based on a single-beam bathymetric sonar and a depth meter, and a suitable search interval is set according to the position provided by the inertial navigation system to ensure that the actual position is within the search area. The size of the search interval is related to the terrain slope. A small search interval is set in places with a large terrain slope, and a larger search interval is set in areas with a small terrain slope, so as to achieve adaptive adjustment of the search area based on the terrain slope. Within the search range, the water depth points of the water depth data in the error measurement interval are extracted from the prior seabed topographic map to obtain the water depth point cloud data corresponding to the observation:

[0097]

[0098] in, Represents the water depth point cloud data extracted within the search area. The generated water depth point cloud data is separated by Euclidean clustering, divided into multiple water depth point cloud regions, and each water depth point cloud region is outsourced using a fully symmetric polytope to generate multiple fully symmetric polytopes corresponding to the observation information. .

[0099] Step 303, measuring and updating the fully symmetric polytope.

[0100] In the embodiment of the present disclosure, the newly generated fully symmetric polytope, i.e., the fully symmetric polytope corresponding to the observed information, is determined. Fully symmetric polytope after motion update Whether there is an intersection, the judgment formula is:

[0101]

[0102] If there is an intersection, the intersection of the two fully symmetric polytopes is calculated. If it does not exist, it is discarded. Then the measured updated multi-fully symmetric polytope is obtained according to the intersection. , as shown below:

[0103]

[0104] in, represents the multi-holosymmetric polytope corresponding to the observed information, represents the fully symmetric polytope at time k, N represents the number of fully symmetric polytopes corresponding to the motion update at this time, and M represents the number of fully symmetric polytopes corresponding to the observation information.

[0105] Step 304, reducing the complexity of the fully symmetric polytope.

[0106] In the disclosed embodiment, it can be found through motion updates and measurement updates that with the increase of iterative steps, the generation matrix of each fully symmetric polytope gradually increases, which will lead to an increase in the computational consumption of the algorithm; at the same time, with the iteration of measurement updates, more and more new fully symmetric polytopes may be generated after the observation update. Too many fully symmetric polytopes will seriously affect the normal operation of the algorithm, so it is necessary to reduce the number and complexity of fully symmetric polytopes.

[0107] In the embodiment of the present disclosure, the number of fully symmetric polytopes obtained after measuring and updating at each moment is calculated. , if the number is greater than the set threshold , it is believed that there are many fully symmetric polytopes at this time, and it is necessary to study the number of fully symmetric polytopes in the system until the number of fully symmetric polytopes reaches the threshold The following. The fully symmetric polytope is a centrally symmetric figure, so the center vector in its formula is the center of the fully symmetric polytope. . Then calculate the distance between the centers of any two fully symmetric polytopes using the following formula:

[0108]

[0109] After calculating the distances between the centers of all fully symmetric polytopes, arrange the distances from small to large, as shown below:

[0110]

[0111] Merge the two closest fully symmetric polytopes and recalculate the center distance of all fully symmetric polytopes until the number of fully symmetric polytopes meets the threshold requirement. Next, calculate the complexity of each fully symmetric polytope, which is related to the dimension of the fully symmetric polytope itself and the dimension of the generator. The system complexity calculation formula is:

[0112]

[0113] in, is the number of generators, and n is the dimension of the system. When the calculated system complexity is greater than the set complexity threshold When the system is considered too complex to be suitable for operation, dimensionality reduction is required. By using the constraint method, the dimensionality reduction problem is transformed into a constrained optimization problem. For fully symmetric polytopes The best approximation of . Through the fully symmetric polytope To achieve To approximate this, we need to calculate the generator matrix , thus ensuring a fully symmetrical polytope As compact as possible. This can be done by minimizing the fully symmetric polytope The principal component analysis method is used to obtain the over-approximation of the fully symmetric polytope , thereby achieving a reduction in the complexity of the fully symmetric polytope.

[0114] Step 305, obtaining an estimated position based on the center, central water depth and volume of the fully symmetric polytope, and correcting the accumulated error of the inertial navigation system.

[0115] In the embodiment of the present disclosure, the above formula can be used to obtain the multi-holosymmetric polytopes after measurement update. After the measurement update is completed, the volume of each holosymmetric polytope needs to be determined. , which is related to the generator matrix of the fully symmetric polytope and is expressed as follows:

[0116]

[0117] in, yes The columns shown in compose the submatrices, Represents the row index.

[0118] The volume of the fully symmetrical polytope is obtained by calculation. Since the weight of the fully symmetrical polytope is directly expressed by the volume, there will be a large area of ​​water depth that is not inside the fully symmetrical polytope. The fully symmetrical polytope occupies a large amount of weight, which leads to errors in the calculation results. Therefore, the weight of each fully symmetrical polytope is obtained based on the measured water depth value, the water depth value at the center of the fully symmetrical polytope, and the volume of the fully symmetrical polytope. The calculation formula is:

[0119]

[0120] in The water depth data representing the center position of the fully symmetric polytope. Therefore, the estimated position is obtained based on the center and weight of the fully symmetric polytope: The estimated position is fed back to the inertial navigation system to correct its accumulated error, and then the motion update and measurement update are executed repeatedly until the end point is reached.

[0121] In the disclosed embodiment, the optimal operating mode is: the update frequency of the proposed navigation method framework algorithm is related to the divergence of the inertial navigation system error over time, the navigation error of the inertial navigation is set to be linearly related to time, the positioning error of the inertial navigation system is estimated according to the instruction manual of the inertial navigation system, and preferably, when the error of the inertial navigation system increases by 20 meters, the terrain navigation method is run once.

[0122] Figure 4 A schematic diagram of a seabed terrain navigation device for an underwater vehicle according to an embodiment of the present disclosure is shown. Figure 3As shown, a seabed terrain navigation device for an underwater vehicle according to an embodiment of the present disclosure includes:

[0123] The information acquisition unit 401 is used to obtain the initial position, attitude and speed information of the aircraft. The inertial navigation system calculates the first position and attitude data of the aircraft based on the attitude, speed information and the initial position, detects the water depth data of the aircraft at the next moment, and the inertial navigation system calculates the second position and attitude data at the next moment based on the water depth data and the attitude and speed information.

[0124] The information acquisition unit 401 is also used to obtain the bottom height, sound speed information and depth information of the aircraft based on the detection device, and to correct the bottom height based on the first position attitude data and the sound speed information; and to calculate the water depth data at the next moment based on the depth information and the corrected bottom height.

[0125] The motion update information acquiring unit 402 is configured to calculate the motion update information based on the first position and posture data and the second position and posture data.

[0126] The motion update information acquisition unit 402 is also used to acquire a priori topographic map of the area where the aircraft is located, perform motion update on the priori topographic map based on the motion update information, and perform error correction on the inertial navigation system based on the priori topographic map after motion update and the water depth data.

[0127] The seabed topographic map establishing unit 403 is used to respectively obtain a rough seabed topographic map and a local fine seabed topographic map, match the local fine seabed topographic map with the corresponding position in the rough seabed topographic map, and establish a variable-scale priori seabed topographic map.

[0128] The fully symmetric polytope updating unit 404 is used to establish an initial fully symmetric polytope based on the initial position and the predicted error range, and to update the initial fully symmetric polytope based on the motion update information and the variable scale prior seabed topography map to obtain a measured updated fully symmetric polytope.

[0129] The fully symmetric polytope updating unit 404 is also used to detect the motion noise of the aircraft and establish a noise fully symmetric polytope based on the motion noise; perform motion update on the initial fully symmetric polytope based on the motion update information and the noise fully symmetric polytope to obtain a motion updated fully symmetric polytope; obtain at least one water depth point area at the location of the aircraft and establish an observation information fully symmetric polytope for each of the water depth point areas; calculate the intersection of the observation information fully symmetric polytope and the motion update fully symmetric polytope, and establish a measurement update fully symmetric polytope based on the intersection. The variable-scale prior seabed topographic map is divided into grids to obtain at least one sub-map; the minimum water depth value, the maximum water depth value and the area side length of each sub-map corresponding area are obtained, and the slope value corresponding to each sub-map is calculated based on the minimum water depth value, the maximum water depth value and the area side length; the inertial navigation system sets a search interval based on the water depth data and the slope value of the current position of the vehicle, and extracts a water depth point data set within the search interval based on the variable-scale prior seabed topographic map; the water depth point data set is segmented based on Euclidean clustering to obtain at least one water depth point area.

[0130] The error correction unit 405 is used to calculate the estimated position of the spacecraft based on the measurement update full symmetric polytope, and correct the accumulated error of the inertial navigation system based on the estimated position.

[0131] The error correction unit 405 is also used to obtain the measured water depth value of the position of the aircraft, and calculate the water depth value of the center position of the measured updated fully symmetrical polytope and the volume of the measured updated fully symmetrical polytope; calculate the weight of the measured updated fully symmetrical polytope based on the measured water depth value, the center position of the measured updated fully symmetrical polytope and the volume and the water depth at the center position; calculate the estimated position of the aircraft based on the weight and the center position of the measured updated fully symmetrical polytope; and correct the accumulated error of the inertial navigation system based on the estimated position.

[0132] The fully symmetric polytope merging unit 406 is used to monitor the number of the measurement updated fully symmetric polytopes in real time, and when the number exceeds a first threshold, obtain the center position of each of the measurement updated fully symmetric polytopes; based on the center position, calculate the center distance between every two of the measurement updated fully symmetric polytopes, sort the calculation results by size, and obtain a first position data set; merge the two measurement updated fully symmetric polytopes corresponding to the smallest center distance in the first position data set, and recalculate the center distance between every two of the measurement updated fully symmetric polytopes to update the first position data set; when the number of the measurement updated fully symmetric polytopes corresponding to the updated first position data set is less than the first threshold, stop merging the measurement updated fully symmetric polytopes.

[0133] In an exemplary embodiment, the information acquisition unit 401, the motion update information acquisition unit 402, the seabed topographic map establishment unit 403, the fully symmetrical polyhedral update unit 404, the error correction unit 405, the fully symmetrical polyhedral merging unit 406, etc. can be implemented by one or more central processing units (CPU, Central Processing Unit), graphics processing units (GPU, Graphics Processing Unit), application specific integrated circuits (ASIC, Application Specific Integrated Circuit), DSP, programmable logic devices (PLD, Programmable Logic Device), complex programmable logic devices (CPLD, Complex Programmable Logic Device), field programmable gate arrays (FPGA, Field-Programmable Gate Array), general-purpose processors, controllers, microcontrollers (MCU, Micro Controller Unit), microprocessors (Microprocessors), or other electronic components.

[0134] Regarding the device in the above embodiment, the specific manner in which each module and unit performs operations has been described in detail in the embodiment of the method, and will not be elaborated here.

[0135] According to an embodiment of the present disclosure, the present disclosure also provides an electronic device and a readable storage medium.

[0136] Figure 5A schematic block diagram of an example electronic device 800 that can be used to implement an embodiment of the present disclosure is shown. The electronic device is intended to represent various forms of digital computers, such as laptop computers, desktop computers, workstations, personal digital assistants, servers, blade servers, mainframe computers, and other suitable computers. The electronic device can also represent various forms of mobile devices, such as personal digital processing, cellular phones, smart phones, wearable devices, and other similar computing devices. The components shown herein, their connections and relationships, and their functions are merely examples and are not intended to limit the implementation of the present disclosure described and / or required herein.

[0137] like Figure 5 As shown, the device 800 includes a computing unit 801, which can perform various appropriate actions and processes according to a computer program stored in a read-only memory (ROM) 802 or a computer program loaded from a storage unit 808 to a random access memory (RAM) 803. In the RAM 803, various programs and data required for the operation of the device 800 can also be stored. The computing unit 801, the ROM 802, and the RAM 803 are connected to each other via a bus 804. An input / output (I / O) interface 805 is also connected to the bus 804.

[0138] A number of components in the device 800 are connected to the I / O interface 805, including: an input unit 806, such as a keyboard, a mouse, etc.; an output unit 807, such as various types of displays, speakers, etc.; a storage unit 808, such as a disk, an optical disk, etc.; and a communication unit 809, such as a network card, a modem, a wireless communication transceiver, etc. The communication unit 809 allows the device 800 to exchange information / data with other devices through a computer network such as the Internet and / or various telecommunication networks.

[0139] The computing unit 801 may be a variety of general and / or special processing components with processing and computing capabilities. Some examples of the computing unit 801 include, but are not limited to, a central processing unit (CPU), a graphics processing unit (GPU), various dedicated artificial intelligence (AI) computing chips, various computing units running machine learning model algorithms, digital signal processors (DSPs), and any appropriate processors, controllers, microcontrollers, etc. The computing unit 801 performs the various methods and processes described above, such as a terrain navigation method for an underwater vehicle. For example, in some embodiments, a terrain navigation method for an underwater vehicle may be implemented as a computer software program, which is tangibly contained in a machine-readable medium, such as a storage unit 808. In some embodiments, part or all of the computer program may be loaded and / or installed on the device 800 via the ROM 802 and / or the communication unit 809. When the computer program is loaded into the RAM 803 and executed by the computing unit 801, one or more steps of the terrain navigation method for an underwater vehicle described above may be performed. Alternatively, in other embodiments, the computing unit 801 may be configured to execute a terrain navigation method for an underwater vehicle in any other appropriate manner (eg, by means of firmware).

[0140] Various implementations of the systems and techniques described above herein can be implemented in digital electronic circuit systems, integrated circuit systems, field programmable gate arrays (FPGAs), application specific integrated circuits (ASICs), application specific standard products (ASSPs), systems on chips (SOCs), load programmable logic devices (CPLDs), computer hardware, firmware, software, and / or combinations thereof. These various implementations can include: being implemented in one or more computer programs that can be executed and / or interpreted on a programmable system including at least one programmable processor, which can be a special purpose or general purpose programmable processor that can receive data and instructions from a storage system, at least one input device, and at least one output device, and transmit data and instructions to the storage system, the at least one input device, and the at least one output device.

[0141] The program code for implementing the method of the present disclosure may be written in any combination of one or more programming languages. These program codes may be provided to a processor or controller of a general-purpose computer, a special-purpose computer, or other programmable data processing device, so that the program code, when executed by the processor or controller, enables the functions / operations specified in the flow chart and / or block diagram to be implemented. The program code may be executed entirely on the machine, partially on the machine, partially on the machine and partially on a remote machine as a stand-alone software package, or entirely on a remote machine or server.

[0142] In the context of the present disclosure, a machine-readable medium may be a tangible medium that may contain or store a program for use by or in conjunction with an instruction execution system, device, or equipment. A machine-readable medium may be a machine-readable signal medium or a machine-readable storage medium. A machine-readable medium may include, but is not limited to, an electronic, magnetic, optical, electromagnetic, infrared, or semiconductor system, device, or device, or any suitable combination of the foregoing. A more specific example of a machine-readable storage medium may include an electrical connection based on one or more lines, a portable computer disk, a hard disk, a random access memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber, a portable compact disk read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination of the foregoing.

[0143] To provide interaction with a user, the systems and techniques described herein can be implemented on a computer having: a display device (e.g., a CRT (cathode ray tube) or LCD (liquid crystal display) monitor) for displaying information to the user; and a keyboard and pointing device (e.g., a mouse or trackball) through which the user can provide input to the computer. Other types of devices can also be used to provide interaction with the user; for example, the feedback provided to the user can be any form of sensory feedback (e.g., visual feedback, auditory feedback, or tactile feedback); and input from the user can be received in any form (including acoustic input, voice input, or tactile input).

[0144] The systems and techniques described herein may be implemented in a computing system that includes back-end components (e.g., as a data server), or a computing system that includes middleware components (e.g., an application server), or a computing system that includes front-end components (e.g., a user computer with a graphical user interface or a web browser through which a user can interact with implementations of the systems and techniques described herein), or a computing system that includes any combination of such back-end components, middleware components, or front-end components. The components of the system may be interconnected by any form or medium of digital data communication (e.g., a communication network). Examples of communication networks include: a local area network (LAN), a wide area network (WAN), and the Internet.

[0145] A computer system may include a client and a server. The client and the server are generally remote from each other and usually interact through a communication network. The relationship of client and server is generated by computer programs running on respective computers and having a client-server relationship with each other. The server may be a cloud server, a server of a distributed system, or a server combined with a blockchain.

[0146] It should be understood that the various forms of processes shown above can be used to reorder, add or delete steps. For example, the steps recorded in this disclosure can be executed in parallel, sequentially or in different orders, as long as the desired results of the technical solutions disclosed in this disclosure can be achieved, and this document does not limit this.

[0147] In addition, the terms "first" and "second" are used for descriptive purposes only and should not be understood as indicating or implying relative importance or implicitly indicating the number of technical features indicated. Thus, a feature defined as "first" or "second" may explicitly or implicitly include at least one of the features. In the description of the present disclosure, the meaning of "plurality" is two or more, unless otherwise clearly and specifically defined.

[0148] The above is only a specific embodiment of the present disclosure, but the protection scope of the present disclosure is not limited thereto. Any person skilled in the art who is familiar with the technical field can easily think of changes or substitutions within the technical scope disclosed in the present disclosure, which should be included in the protection scope of the present disclosure. Therefore, the protection scope of the present disclosure should be based on the protection scope of the claims.

Claims

1. A seabed terrain navigation method for an underwater vehicle, characterized in that: The method comprises: Acquiring the initial position, attitude and speed information of the aircraft, and the inertial navigation system calculating the first position attitude data of the aircraft based on the attitude, the speed information and the initial position; Detecting water depth data of the aircraft at the next moment, wherein the inertial navigation system calculates second position and attitude data at the next moment based on the water depth data, the attitude, and the speed information; Calculate motion update information based on the first position and posture data and the second position and posture data; Acquire a rough seafloor topographic map and a local fine seafloor topographic map respectively, match the local fine seafloor topographic map with the corresponding position in the rough seafloor topographic map, and establish a variable-scale prior seafloor topographic map; An initial fully symmetric polytope is established based on the initial position and the predicted error range, and the initial fully symmetric polytope is updated based on the motion update information and the variable scale prior seafloor topography map to obtain a measured updated fully symmetric polytope; Calculating an estimated position of the vehicle based on the measurement-updated fully symmetric polytope, and correcting a cumulative error of the inertial navigation system based on the estimated position; wherein, detecting the motion noise of the aircraft, and establishing a noise fully symmetric polytope based on the motion noise; Performing motion updating on the initial fully symmetric polytope based on the motion update information and the noise fully symmetric polytope to obtain a motion updated fully symmetric polytope; Acquire at least one water depth point area where the aircraft is located, and establish a fully symmetric polytope of observation information for each of the water depth point areas; Calculate the intersection of the observation information fully symmetric polytope and the motion update fully symmetric polytope, and establish the measurement update fully symmetric polytope based on the intersection; wherein, the measured water depth value of the position of the aircraft is obtained, and the water depth value of the center position of the measured updated fully symmetric polytope and the volume of the measured updated fully symmetric polytope are calculated; Calculate the weight of the measured updated fully symmetric polytope based on the measured water depth value, the center position of the measured updated fully symmetric polytope, the volume and the water depth value at the center position, and calculate the estimated position of the aircraft based on the weight and the center position of the measured updated fully symmetric polytope; The accumulated error of the inertial navigation system is corrected based on the estimated position.

2. The method according to claim 1, characterized in that: The detecting of the water depth data of the aircraft at the next moment comprises: Acquire the bottom altitude, sound speed information and depth information of the aircraft based on the detection device, and correct the bottom altitude based on the first position attitude data and the sound speed information; The water depth data at the next moment is calculated based on the depth information and the corrected bottom height.

3. The method according to claim 1, characterized in that The performing error correction on the inertial navigation system based on the motion update information comprises: A priori topographic map of the area where the aircraft is located is obtained, the priori topographic map is motion updated based on the motion update information, and the inertial navigation system is error corrected based on the priori topographic map after motion update and the water depth data.

4. The method according to claim 1, characterized in that The step of obtaining at least one water depth point area of ​​the location of the aircraft comprises: Dividing the variable-scale priori seafloor topographic map into grids to obtain at least one sub-map; Obtaining a minimum water depth value, a maximum water depth value, and a region side length of each region corresponding to the sub-map, and calculating a slope value corresponding to each sub-map based on the minimum water depth value, the maximum water depth value, and the region side length; The inertial navigation system sets a search interval based on the water depth data and the slope value of the current position of the vehicle, and extracts a water depth point data set within the search interval based on the variable-scale priori seafloor topographic map; The water depth point data set is segmented based on Euclidean clustering to obtain at least one water depth point area.

5. The method according to claim 1, characterized in that The method further comprises: monitoring the number of the measured updated fully symmetric polytopes in real time, and when the number exceeds a first threshold, obtaining the center position of each of the measured updated fully symmetric polytopes; Based on the center position, calculate the center distance between every two of the measured updated fully symmetric polytopes, sort the calculation results by size, and obtain a first position data set; Merging the two measured updated fully symmetric polytopes corresponding to the smallest center distance in the first position data set, and recalculating the center distance between every two measured updated fully symmetric polytopes to update the first position data set; When the number of the measurement update fully symmetric polytopes corresponding to the updated first position data set is less than the first threshold, stopping the merging of the measurement update fully symmetric polytopes; The complexity of each obtained fully symmetric polytope is calculated. When the complexity exceeds a second threshold, a complexity reduction operation is performed on the fully symmetric polytope to reduce the algorithm calculation consumption.

6. A seabed terrain navigation device for an underwater vehicle, characterized in that: The device comprises: An information acquisition unit, used to acquire the initial position, attitude and speed information of the aircraft, wherein the inertial navigation system calculates first position attitude data of the aircraft based on the attitude, the speed information and the initial position, detects the water depth data of the aircraft at the next moment, and the inertial navigation system calculates second position attitude data at the next moment based on the water depth data and the attitude and the speed information; a motion update information acquisition unit, configured to calculate motion update information based on the first position and posture data and the second position and posture data; A seabed topographic map establishing unit, used to respectively obtain a rough seabed topographic map and a local fine seabed topographic map, match the local fine seabed topographic map with the corresponding position in the rough seabed topographic map, and establish a variable-scale priori seabed topographic map; A fully symmetric polytope updating unit is used to establish an initial fully symmetric polytope based on the initial position and the predicted error range, and to update the initial fully symmetric polytope based on the motion update information and the variable scale prior seafloor topography map to obtain a measured updated fully symmetric polytope; an error correction unit, configured to calculate an estimated position of the vehicle based on the measured updated fully symmetric polytope, and to correct a cumulative error of the inertial navigation system based on the estimated position; The fully symmetric polytope updating unit is also used to detect the motion noise of the aircraft, and establish a noise fully symmetric polytope based on the motion noise; perform motion update on the initial fully symmetric polytope based on the motion update information and the noise fully symmetric polytope to obtain a motion updated fully symmetric polytope; obtain at least one water depth point area where the aircraft is located, and establish an observation information fully symmetric polytope for each of the water depth point areas; calculate the intersection of the observation information fully symmetric polytope and the motion updated fully symmetric polytope, and establish a measurement updated fully symmetric polytope based on the intersection; The error correction unit is further used to obtain the measured water depth value of the position of the aircraft, and calculate the water depth value of the center position of the measured updated fully symmetric polytope and the volume of the measured updated fully symmetric polytope; based on the measured water depth value, the center position of the measured updated fully symmetric polytope, the volume and the water depth value at the center position, calculate the weight of the measured updated fully symmetric polytope, and calculate the estimated position of the aircraft based on the weight and the center position of the measured updated fully symmetric polytope; The accumulated error of the inertial navigation system is corrected based on the estimated position.

7. An electronic device, characterized in that: include: at least one processor; as well as a memory communicatively connected to the at least one processor; wherein, The memory stores instructions that can be executed by the at least one processor, and the instructions are executed by the at least one processor to enable the at least one processor to perform the method according to any one of claims 1 to 5.

8. A non-transitory computer-readable storage medium storing computer instructions, characterized in that: The computer instructions are used to make a computer execute the method according to any one of claims 1-5.

Images