A method and system for underwater laser terrain matching navigation in deep sea

Abstract

The present invention provides an underwater laser terrain matching navigation method and system for the deep sea, which is applied to a submersible and belongs to the field of deep sea exploration. The method of the present invention comprises: obtaining a seabed terrain database and the position and posture information, speed information, depth information, heading information and current terrain data map of the submersible; preprocessing and trajectory splicing the current terrain data map; matching and aligning the processed terrain data map with the seabed terrain database through an iterative closest point algorithm; and using the rotation matrix and translation vector obtained by the matching and alignment to correct the accumulated position and posture error information of the submersible. The present invention obtains a seabed terrain data map through an underwater laser detector, and combines it with an underwater laser terrain matching navigation method for the deep sea, so as to effectively improve the matching accuracy of the seabed terrain matching navigation system, and meet the requirements of reliable high-precision navigation and positioning for the long-term operation of the submersible underwater.

Description

Technical Field

[0001] The present invention relates to the field of deep sea exploration, and in particular to an underwater laser terrain matching navigation method and system for deep sea. Background Art

[0002] With the rapid development of marine science research, the exploration and development of deep-sea resources, the detailed mapping of seabed topography, and the high-precision navigation of deep-sea equipment have gradually increased. In particular, in the fields of deep-sea engineering, ecological monitoring, and national defense security, higher requirements are placed on underwater positioning technology. However, due to the complex deep-sea environment and limited signal propagation, the current mainstream positioning technology still faces many challenges.

[0003] Inertial navigation systems achieve self-positioning by measuring the movement changes of devices using accelerometers and gyroscopes. However, their errors accumulate over time, which is the so-called "drift problem." Although other navigation technologies can be combined, high-frequency signals decay quickly in environments such as the deep sea, so conventional positioning methods such as GPS are not applicable, and the accuracy of inertial navigation is difficult to guarantee.

[0004] Sonar is currently the most commonly used underwater detection tool, which measures the position of the target object by emitting sound wave signals and receiving reflected echoes. However, since sound waves are greatly affected by noise, scattering, attenuation, etc. when propagating in water, its detection resolution is limited, especially in complex terrain or high-precision operation environments. In addition, sonar technology relies on acoustic arrays and usually requires fixed or towed base station equipment, which has high deployment costs and lacks flexibility.

[0005] Although traditional sonar technology has certain advantages in underwater detection, its accuracy is low and cannot meet the needs of certain refined operations. In recent years, laser detection technology has become an emerging means of underwater detection. Its high resolution and high precision provide new possibilities for underwater positioning. However, how to efficiently match the point cloud data obtained by laser detection with the existing seabed map has become a key technical issue in achieving high-precision underwater positioning. Summary of the invention

[0006] According to the technical problems raised above, a method and system for underwater laser terrain matching navigation in the deep sea is provided. The present invention mainly includes an inertial navigation system, a Doppler velocity log, a depth meter and an underwater laser detector. In combination with a seabed terrain database, the inertial navigation system and terrain detection equipment are used to provide initial geographic location information as a reference, and the seabed terrain data is collected in real time by relying on ship-borne terrain measurement sensors. The collected real-time terrain data is matched with the pre-stored underwater digital terrain map, so as to correct the accumulated error of inertial navigation, and finally achieve high-precision underwater positioning and navigation capabilities.

[0007] The technical means adopted by the present invention are as follows:

[0008] An underwater laser terrain matching navigation method for deep sea, applied in a submersible, comprising:

[0009] Obtain the seabed terrain database and the position information, speed information, depth information, heading information and current terrain data map of the submersible;

[0010] Preprocess the current terrain data map and splice the trajectory;

[0011] The processed topographic data map is matched and registered with the seabed topography database through an iterative closest point algorithm;

[0012] The accumulated position and attitude error information of the submersible is corrected using the rotation matrix and translation vector obtained by matching and registration.

[0013] Furthermore, the position information is obtained by superimposing the position information and attitude information of the submersible; and the speed information is obtained by integrating the acceleration of the submersible.

[0014] Furthermore, the preprocessing and trajectory splicing of the current terrain data map specifically includes:

[0015] Downsampling the current terrain data map through voxel filtering; determining the downsampled data by setting the voxel side length, and removing the redundancy of the point cloud data;

[0016] The preprocessed terrain data map is tracked and spliced ​​to obtain a terrain data map with a complete perspective.

[0017] Furthermore, the iterative closest point algorithm specifically includes:

[0018] The processed terrain data map is used as the source point cloud , taking the seabed topography basic database as the target point cloud , the source point cloud By translating and rotating the target point cloud Registration;

[0019] Calculate the rotation matrix of the data without error and translation vector ,from Coordinate system goes to The formula for the coordinate system is:

[0020] ;

[0021] in, Source point cloud The elements, Target point cloud The elements;

[0022] Minimize the objective function:

[0023] ;

[0024] Iterate the rotation matrix by SVD decomposition and translation vector , the centroids of the two groups of point clouds are expressed as:

[0025] ;

[0026] ;

[0027] in, is the centroid of the source point cloud, is the centroid of the target point cloud. The target point cloud and the source point cloud are expressed in the centroid coordinate system as , ;

[0028] Rotation Matrix and translation vector To optimize, set and The optimal solution is:

[0029] ;

[0030] ;

[0031] Let the covariance matrix , through Do SVD decomposition and we get , and is an orthogonal matrix, when When full rank, , corresponding to the only , Combine and get the corresponding and :

[0032] ;

[0033] ;

[0034] in, , , Respectively The elements on the main diagonal are The singular values ​​of

[0035] Get the transformation matrix , in the absence of error, .

[0036] Furthermore, the correction of the accumulated posture error information of the submersible specifically includes:

[0037] The transformation matrix obtained by iterative closest point algorithm , correct the position vector calculated by the combined navigation system in the submersible under the navigation system , and obtain the corrected position vector :

[0038] ;

[0039] ;

[0040] in, Represents the direction cosine matrix from the underwater laser coordinate system to the navigation coordinate system, Represents the direction cosine matrix transformed from the navigation coordinate system to the underwater laser coordinate system.

[0041] The present invention also includes an underwater laser terrain matching navigation system for deep sea, which is implemented based on the underwater laser terrain matching navigation method for deep sea, comprising: an inertial navigation system, a Doppler velocity log, a depth meter and an underwater laser detector, wherein:

[0042] The inertial navigation system is used to obtain the position and posture information of the submersible;

[0043] The Doppler speed log is used to obtain speed information of the submersible;

[0044] The depth meter is used to obtain the depth information of the submersible;

[0045] The underwater laser detector is used to obtain the current terrain data map of the submersible.

[0046] Compared with the prior art, the present invention has the following advantages:

[0047] The underwater laser terrain matching navigation method and system for deep sea provided by the present invention, the underwater laser detector can generate high-resolution point cloud data, and its spatial resolution is higher than the point cloud data generated by traditional sonar. The underwater laser detector can capture more subtle seabed terrain details, thereby providing centimeter-level or even millimeter-level terrain measurement accuracy. High-precision terrain data provides basic data for matching complex seabed terrain. The underwater laser detector can clearly capture complex terrain features, such as details such as seabed faults, gullies, rock structures, etc., and can still provide accurate point cloud data even in areas with large undulating terrain or dense terrain features, greatly improving the effect of terrain matching navigation. The underwater laser detector has a wide scanning coverage range and a fast scanning speed, and can provide high-precision seabed terrain data, and can complete high-resolution terrain of a large area of ​​seabed in a short time. Underwater laser detection has strong adaptability to complex terrain, especially in areas where underwater sonar is limited, such as steep seabed mountains or flat sedimentary areas, using underwater laser detectors for scanning can more accurately and finely detect terrain features.

[0048] The underwater laser terrain matching navigation method and system for deep sea provided by the present invention have the problem of cumulative errors of inertial navigation system and Doppler log during long-term operation of the submersible. Through real-time laser scanning and terrain matching, these cumulative errors can be effectively corrected to improve the long-term stability and accuracy of positioning.

[0049] The underwater laser terrain matching navigation method and system for deep sea provided by the present invention uses an iterative closest point algorithm to match directly based on point cloud data, without the need to explicitly extract feature points or surface information. By accurately matching these high-resolution point clouds, through the two main steps of nearest point search and rigid body transformation solution, it is continuously iterated until convergence to the optimal solution, and finally millimeter-level or even centimeter-level positioning accuracy can be achieved in complex underwater terrain. In addition, the iterative closest point algorithm has a simple structure, and the calculation process mainly includes nearest point search and matrix solution, which is suitable for real-time operation. Combined with high-resolution laser point clouds, matching can be completed in a short time to meet the real-time requirements of underwater navigation. Terrain matching navigation is a passive positioning method that relies entirely on seabed terrain data and is not restricted by external signals. The iterative closest point algorithm can independently achieve precise positioning by matching real-time point clouds with reference maps, and is suitable for environments such as the deep sea where external signals cannot cover.

[0050] The underwater laser terrain matching navigation method and system for deep sea provided by the present invention have significant terrain differences in the seabed terrain, which may include dramatically undulating mountains and gullies as well as relatively flat sedimentary areas. In addition, due to light scattering, reflection interference and the limitation of laser penetration depth, point cloud data may contain noise and partial missing. The matching principle of the iterative closest point algorithm is based on the overall structure of the point cloud rather than local features, and can still converge to a reasonable solution even in the presence of more noise. Therefore, it has strong adaptability and robustness to complex terrain.

[0051] Based on the above reasons, the present invention can be widely promoted in the fields of deep sea exploration and the like. BRIEF DESCRIPTION OF THE DRAWINGS

[0052] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative labor.

[0053] Figure 1 This is a flow chart of the underwater laser terrain matching navigation method used in the deep sea in the present invention.

[0054] Figure 2 This is a structural diagram of the underwater laser terrain matching navigation system used in the deep sea in the present invention.

[0055] Figure 3 This is an example diagram of underwater laser scanning of terrain by a submersible in an embodiment of the present invention.

[0056] Figure 4 This is a terrain depth reference map after underwater laser scanning by a submersible in an embodiment of the present invention.

[0057] Figure 5 It is a schematic diagram of the position information of the submersible after the position is corrected by underwater laser terrain matching in an embodiment of the present invention.

[0058] Figure 6 This is a real-time scanning terrain depth map by the submersible in an embodiment of the present invention. DETAILED DESCRIPTION

[0059] It should be noted that, in the absence of conflict, the embodiments of the present invention and the features in the embodiments can be combined with each other. The present invention will be described in detail below with reference to the accompanying drawings and in combination with the embodiments.

[0060] In order to make the purpose, technical solutions and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. The following description of at least one exemplary embodiment is actually only illustrative and is by no means intended to limit the present invention and its application or use. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0061] It should be noted that the terms used herein are only for describing specific embodiments and are not intended to limit exemplary embodiments according to the present invention. As used herein, unless the context clearly indicates otherwise, the singular form is also intended to include the plural form. In addition, it should be understood that when the terms "comprising" and / or "including" are used in this specification, it indicates the presence of features, steps, operations, devices, components and / or combinations thereof.

[0062] Unless otherwise specifically stated, the relative arrangement of the parts and steps described in these embodiments, the numerical expressions and numerical values ​​do not limit the scope of the present invention. At the same time, it should be clear that, for ease of description, the sizes of the various parts shown in the drawings are not drawn according to the actual proportional relationship. The technology, methods and equipment known to ordinary technicians in the relevant field may not be discussed in detail, but in appropriate cases, the technology, methods and equipment should be regarded as part of the authorization specification. In all examples shown and discussed here, any specific value should be interpreted as merely exemplary, rather than as a limitation. Therefore, other examples of exemplary embodiments may have different values. It should be noted that similar numbers and letters represent similar items in the following drawings, so once an item is defined in one drawing, it does not need to be further discussed in subsequent drawings.

[0063] In the description of the present invention, it is necessary to understand that the directions or positional relationships indicated by directional words such as "front, back, up, down, left, right", "lateral, vertical, perpendicular, horizontal" and "top, bottom" are usually based on the directions or positional relationships shown in the drawings. They are only for the convenience of describing the present invention and simplifying the description. Unless otherwise specified, these directional words do not indicate or imply that the device or element referred to must have a specific direction or be constructed and operated in a specific direction. Therefore, they cannot be understood as limiting the scope of protection of the present invention: the directional words "inside and outside" refer to the inside and outside relative to the contours of each component itself.

[0064] For ease of description, spatially relative terms such as "above", "above", "on the upper surface of", "above", etc. may be used here to describe the spatial positional relationship between a device or feature and other devices or features as shown in the figure. It should be understood that spatially relative terms are intended to include different orientations of the device in use or operation in addition to the orientation described in the figure. For example, if the device in the accompanying drawings is inverted, the device described as "above other devices or structures" or "above other devices or structures" will be positioned as "below other devices or structures" or "below their position devices or structures". Thus, the exemplary term "above" may include both "above" and "below". The device may also be positioned in other different ways (rotated 90 degrees or in other orientations), and the spatially relative descriptions used here are interpreted accordingly.

[0065] In addition, it should be noted that the use of terms such as "first" and "second" to limit components is only for the convenience of distinguishing the corresponding components. If not otherwise stated, the above terms have no special meaning and therefore cannot be understood as limiting the scope of protection of the present invention.

[0066] like Figure 1 As shown, the present invention provides an underwater laser terrain matching navigation method for deep sea, which is applied to a submersible and is characterized by comprising:

[0067] Obtain the seabed terrain database and the position information, speed information, depth information, heading information and current terrain data map of the submersible;

[0068] In specific implementation, as a preferred embodiment of the present invention, the position information is obtained by superimposing the position information and attitude information of the submersible; and the speed information is obtained by integrating the acceleration of the submersible.

[0069] During implementation, the acceleration obtained by the accelerometer in the inertial navigation system is double-integrated and the vehicle speed information provided by the Doppler velocity log is integrated to obtain the position information of the vehicle, the data obtained by the gyroscope in the inertial navigation system is integrated to obtain the attitude information of the vehicle, and finally the position information is superimposed on the attitude information to obtain the position information of the vehicle.

[0070] Preprocess the current terrain data map and splice the trajectory;

[0071] In specific implementation, as a preferred embodiment of the present invention, the preprocessing and trajectory splicing of the current terrain data map specifically includes:

[0072] Downsampling the current terrain data map through voxel filtering; determining the downsampled data by setting the voxel side length, and removing the redundancy of the point cloud data;

[0073] The preprocessed terrain data map is tracked and spliced ​​to obtain a terrain data map with a complete perspective.

[0074] The processed topographic data map is matched and registered with the seabed topography database through an iterative closest point algorithm;

[0075] In specific implementation, as a preferred embodiment of the present invention, the iterative closest point algorithm specifically includes:

[0076] The processed terrain data map is used as the source point cloud , taking the seabed topography basic database as the target point cloud , the source point cloud By translating and rotating the target point cloud Registration;

[0077] Calculate the rotation matrix of the data without error and translation vector ,from Coordinate system goes to The formula for the coordinate system is:

[0078] ;

[0079] in, Source point cloud The elements, Target point cloud The elements;

[0080] In the implementation, due to the presence of noise and point mismatching, and The point cloud data is mistakenly considered to be the same point by the feature matching algorithm, so the objective function needs to be minimized:

[0081] ;

[0082] Iterate the rotation matrix by SVD decomposition and translation vector , the centroids of the two groups of point clouds are expressed as:

[0083] ;

[0084] ;

[0085] in, is the centroid of the source point cloud, is the centroid of the target point cloud. The target point cloud and the source point cloud are expressed in the centroid coordinate system as , ;

[0086] Rotation Matrix and translation vector To optimize, set and The optimal solution is:

[0087] ;

[0088] ;

[0089] Let the covariance matrix , through Do SVD decomposition and we get , and is an orthogonal matrix, when When full rank, , corresponding to the only , Combine and get the corresponding and :

[0090] ;

[0091] ;

[0092] in, , , Respectively The elements on the main diagonal are The singular values ​​of

[0093] Get the transformation matrix , in the absence of error, .

[0094] The accumulated position and attitude error information of the submersible is corrected using the rotation matrix and translation vector obtained by matching and registration.

[0095] In specific implementation, as a preferred embodiment of the present invention, the correction of the accumulated posture error information of the submersible specifically includes:

[0096] The transformation matrix obtained by iterative closest point algorithm , correct the position vector calculated by the combined navigation system in the submersible under the navigation system , and obtain the corrected position vector :

[0097] ;

[0098] ;

[0099] in, Represents the direction cosine matrix from the underwater laser coordinate system to the navigation coordinate system, Represents the direction cosine matrix transformed from the navigation coordinate system to the underwater laser coordinate system.

[0100] Corresponding to the underwater laser terrain matching navigation method for deep sea in the present invention, it also includes an underwater laser terrain matching navigation system for deep sea, including: an inertial navigation system, a Doppler velocity log, a depth meter and an underwater laser detector, wherein:

[0101] The inertial navigation system is used to obtain the position and posture information of the submersible;

[0102] The Doppler speed log is used to obtain speed information of the submersible;

[0103] The depth meter is used to obtain the depth information of the submersible;

[0104] The underwater laser detector is used to obtain the current terrain data map of the submersible.

[0105] Example

[0106] like Figure 1 and Figure 2 As shown, the present invention provides an underwater laser terrain matching navigation method and system for deep sea. At present, the underwater laser detector adopts high-resolution three-dimensional imaging technology, which can work in complex seabeds and under severe multi-path signal conditions to complete the fine measurement of seabed topography. The underwater laser detector has the advantages of small size, light weight and low power consumption. At the same time, it can perform three-dimensional scanning at a fixed position. It is particularly suitable for installation on underwater towed bodies, underwater submersibles, remote-controlled submersibles and manned submersibles. The emergence of underwater laser detectors provides a more accurate matching map basis for deep-sea terrain matching auxiliary navigation and positioning. The underwater three-dimensional terrain image can clearly present various different terrains on the seabed surface, and the rich terrain features provide a selection space for terrain matching, thereby greatly improving the matching success rate, accuracy and stability.

[0107] The seabed topography basic database is obtained through historical data measured by underwater laser detectors in the early stage. If the synchronous positioning and map building technology is adopted, the seabed topography basic database measured in the early stage may not be needed, but the seabed topography basic database can be established in real time through underwater laser detectors during the actual navigation and positioning process.

[0108] The inertial navigation system and Doppler velocity log are the basis of the entire navigation and positioning system. They are used to provide initial position estimation information and limit the range of the underwater laser detector in the submersible to scan the terrain.

[0109] Underwater laser detectors are used to synchronously measure the seabed topography within a certain range below the submersible and provide real-time seabed topographic maps.

[0110] According to the planned mission and the planned route of the submersible, this embodiment selects a submarine terrain database within a certain range near the planned route from the existing submarine terrain database, performs grid processing, and stores it in the submersible as a reference map of underwater terrain and landforms, such as Figure 3 shown.

[0111] The submersible navigates along the predetermined route under the guidance of the inertial navigation system.

[0112] During the voyage, the underwater laser detector is turned on to synchronously measure the seabed topography data below the submersible, such as Figure 4 As shown, it is pre-processed and stored.

[0113] Select the seabed topography data map at the current time and within a certain time or track range before, that is, obtain the real-time topography data map of a certain area, process the measured topography data map of the area, and generate a real-time seabed topography map, such as Figure 6 shown.

[0114] Based on the position information given by the inertial navigation system, the matching range is selected from the seabed terrain database to generate the optimal matching position of the real-time seabed terrain map.

[0115] The seabed terrain matching results are used to correct the divergence of the inertial navigation system errors, such as Figure 5 As shown, the deep seabed terrain matching auxiliary navigation positioning at the current moment is completed.

[0116] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or replace some or all of the technical features therein with equivalents. However, these modifications or replacements do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of the present invention.

Claims

1. An underwater laser terrain matching navigation method for deep sea, applied to a submersible, characterized in that: include: Obtain the seabed terrain database and the position information, speed information, depth information, heading information and current terrain data map of the submersible; The position and attitude information is obtained by superimposing the position information and attitude information of the submersible; The speed information is obtained by integrating the acceleration of the submersible; The acceleration obtained by the accelerometer in the inertial navigation system is double-integrated with the vehicle speed information provided by the Doppler velocity log to obtain the position information of the vehicle, the data obtained by the gyroscope in the inertial navigation system is integrated to obtain the attitude information of the vehicle, and finally the position information is superimposed on the attitude information to obtain the position and posture information of the vehicle; Preprocess the current terrain data map and splice the trajectory, including: The current terrain data map is downsampled by voxel filtering; the downsampled data is determined by setting the voxel side length, and the point cloud data redundancy is removed; the pre-processed terrain data map is track spliced ​​to obtain a terrain data map with a complete perspective; The processed topographic data map is matched and registered with the seabed topography database through an iterative closest point algorithm; The iterative closest point algorithm specifically includes: The processed terrain data map is used as the source point cloud , taking the seabed topography basic database as the target point cloud , the source point cloud By translating and rotating the target point cloud Registration; Calculate the rotation matrix of the data without error and translation vectors ,from Coordinate system goes to The formula for the coordinate system is: ； in, Source point cloud The elements, Target point cloud The elements; Minimize the objective function: ； Iterate the rotation matrix by SVD decomposition and translation vectors , the centroids of the two groups of point clouds are expressed as: ； ； in, is the centroid of the source point cloud, is the centroid of the target point cloud. The target point cloud and the source point cloud are expressed in the centroid coordinate system as , ; Rotation Matrix and translation vectors Optimize and set and The optimal solution is: ； ； Let the covariance matrix , through Do SVD decomposition, we get , and is an orthogonal matrix, when When full rank, , corresponding to the only , Combine and get the corresponding and : ； ； in, , , Respectively The elements on the main diagonal are The singular values ​​of Get the transformation matrix , in the absence of error, ; The accumulated position and attitude error information of the submersible is corrected using the rotation matrix and translation vector obtained by matching and registration; The correction of the accumulated attitude error information of the submersible specifically includes: The transformation matrix obtained by iterative closest point algorithm , correct the position vector calculated by the combined navigation system in the submersible under the navigation system , and obtain the corrected position vector : ； ； in, Represents the direction cosine matrix from the underwater laser coordinate system to the navigation coordinate system, Represents the direction cosine matrix transformed from the navigation coordinate system to the underwater laser coordinate system.

2. An underwater laser terrain matching navigation system for deep sea implemented based on the underwater laser terrain matching navigation method for deep sea described in claim 1, characterized in that: include: Inertial navigation systems, Doppler velocity logs, depth gauges and underwater laser sounders, including: The inertial navigation system is used to obtain the position and posture information of the submersible; The Doppler speed log is used to obtain speed information of the submersible; The depth meter is used to obtain the depth information of the submersible; The underwater laser detector is used to obtain the current terrain data map of the submersible.

Images