Target three-dimensional positioning method applied to underwater vehicle
By combining the surface constraint and spherical constraint models of forward-view sonar images on the underwater vehicle and combining navigation system data, high-precision three-dimensional positioning in the deep-sea environment is achieved, and the positioning difficulties of forward-view sonar in the sparse feature environment are solved. and noise impact problems, improve processing speed and stability, and are suitable for a variety of marine exploration scenarios.
Patent Information
- Application Number
- CN202411190251.0
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-08-28
- Publication Date
- 2024-11-29
AI Technical Summary
The existing underwater SLAM technology based on forward-view sonar is difficult to achieve precise positioning in deep-sea environments with sparse characteristics, and the noise in forward-view sonar images affects the three-dimensional reconstruction effect, limiting the large-scale navigation of underwater submarines and the positioning of submarine targets. ability.
The surface constraint and spherical constraint model based on the forward-view sonar image are adopted, combined with the position data of the underwater submarine navigation system, and the average value calculation of multiple observation data can be calculated to achieve the three-dimensional positioning of the target, reducing the impact of noise on accuracy, and is suitable for features Sparse and rich environment.
Achieve high-precision three-dimensional positioning in deep-sea environments with sparse characteristics, improve processing speed and stability, reduce learning costs and operational complexity, and is suitable for a variety of marine exploration scenarios.
Smart Images

Figure CN119044977A_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to the field of ocean detection technology, and more specifically to a target three-dimensional positioning method applied to an underwater vehicle. Background Art
[0002] In the field of modern ocean exploration, forward-looking sonar technology has become a key tool for underwater vehicles to perform various tasks such as target detection, seabed mapping and navigation due to its unique advantages, such as long-distance detection, no influence from water turbidity, and real-time two-dimensional imaging. Among them, the use of forward-looking sonar systems to search and locate specific ocean targets is one of its most common applications.
[0003] At present, the research on positioning based on forward-looking sonar mainly focuses on the positioning of the sonar itself, while trying to map the surrounding sea area. In underwater hull inspection, the simultaneous localization and mapping (SLAM) technology of forward-looking sonar is committed to solving the problem of sparse sonar image features and association of false alarm data, and usually uses graph optimization methods to solve the positioning problem. In addition, some studies have combined inertial navigation data and forward-looking sonar images for fusion positioning, using an inertial-assisted sliding window optimization framework to better deal with the problem of erroneous feature matching or insufficient features. Another method proposes an underwater robot exploration framework suitable for operation in cluttered environments. The framework is based on the simultaneous localization and mapping (SLAM) implementation of carrying imaging sonar, and includes multiple parts such as path generation, location recognition prediction, belief propagation, and utility evaluation using virtual maps.
[0004] However, there is a significant limitation of underwater SLAM technology based on forward-looking sonar, which requires rich feature information in the operating environment. This means that these technologies can only be used in places with rich features, such as ports. Deep sea areas far from the shore often have flat seabeds, and seabed objects and features are sparse, resulting in a lack of sufficient information for forward-looking sonar SLAM to achieve precise positioning in such an environment, limiting the ability of underwater submersibles to perform large-scale navigation. At the same time, this limitation reduces the application potential of forward-looking sonar SLAM in seabed target positioning tasks.
[0005] For the study of ocean 3D reconstruction, some methods focus on related work such as motion structure reconstruction (SFM) using forward-looking sonar images. For example, a probabilistic 3D occupancy mapping framework of forward-looking sonar images tailored for underwater submersibles is used for 3D reconstruction, which can reconstruct the seabed terrain structure. There are also studies that apply the improved SFS algorithm in optics to the 3D reconstruction of 2D sonar images, and verify through simulation and sea trial data that the results of the algorithm reconstruction are closer to the actual size of the object.
[0006] Nevertheless, the research on 3D reconstruction of seabed objects based on forward-looking sonar shows that this requires high-quality imaging of the object by the forward-looking sonar, and the features of the object must be obvious. If the forward-looking sonar image contains a lot of noise, the effect of 3D reconstruction will be greatly reduced. However, in real ocean exploration situations, forward-looking sonar images often have a lot of noise and blur.
[0007] Therefore, how to solve the above technical problems and break through the above bottlenecks is an issue that the current technical personnel in this field urgently need to solve. Summary of the invention
[0008] In view of this, in order to overcome the above-mentioned defects, the present invention provides a method for three-dimensional positioning of targets for underwater vehicles. The method can be applied in deep-sea environments with sparse features, and can quickly and accurately achieve three-dimensional positioning of targets, which not only overcomes the limitations of the existing technology in terms of accuracy, noise processing, processing speed, adaptability and ease of operation, but also brings significant technical advantages to the field of marine exploration.
[0009] In order to achieve the above object, the present invention adopts the following technical solution:
[0010] A target three-dimensional positioning method applied to an underwater vehicle, comprising:
[0011] After identifying the target in the sonar image, the surface constraints of the target are determined based on the position coordinates of the underwater vehicle corresponding to the current target.
[0012] Based on the surface constraint at the first moment and the surface constraint at the second moment, a constraint model is constructed, the constraint model is solved, and the target is three-dimensionally positioned.
[0013] Preferably, the time interval between the first moment and the second moment is greater than a set threshold value to avoid positioning the target based on data corresponding to two consecutive moments; because the change in the observation angle of the same object through two consecutive moments is very small, resulting in a poor positioning effect;
[0014] In this application, the threshold value can be determined as needed.
[0015] Preferably, the surface constraint includes a plane constraint and a spherical constraint; the constraint model includes a plane constraint at a first moment, a plane constraint at a second moment, and a spherical constraint at the first moment, or a plane constraint at the first moment, a plane constraint at the second moment, and a spherical constraint at the second moment.
[0016] Preferably, when solving the constraint model, the solution corresponding to the larger z coordinate in the obtained solution is used as the target position.
[0017] Preferably, the expression of the plane constraint is:
[0018] Akn X+B kn Y+C kn Z+D kn =0
[0019] In the formula, A kn , B kn , C kn and D kn represents the coefficients of the plane equation, where
[0020]
[0021] In the formula, is the normal vector of the plane, (X k , Y k , Z k ) is the position coordinate of the underwater vehicle at time k; and
[0022]
[0023] In the formula, R k is the rotation matrix of the sonar coordinate system relative to the world coordinate system, θ kn is the azimuth angle θ of the target in the sonar coordinate system kn .
[0024] Preferably, identifying the target in the sonar image and recording the underwater vehicle position information corresponding to the target at the current moment to form target observation data;
[0025] Randomly extract observation data at any two different times, perform multiple three-dimensional positioning, and take the average of multiple positioning results as the target three-dimensional positioning result.
[0026] Preferably, the underwater vehicle is equipped with a forward-looking sonar and a navigation system, the forward-looking sonar is used to scan and obtain sonar images and identify targets in the sonar images, and the navigation system is used to provide the position coordinates of the underwater vehicle at different times.
[0027] Through the above technical solutions, it can be seen that the present invention discloses a method for three-dimensional positioning of targets applied to underwater vehicles. By combining the forward-looking sonar image registration results and the attitude and positioning data output by the underwater vehicle navigation system, the three-dimensional positioning of the target can be quickly achieved when the underwater vehicle performs autonomous scanning tasks, and the three-dimensional positioning results are excellent. Compared with the prior art,
[0028] The present invention can achieve high-precision three-dimensional positioning in deep-sea environments with sparse features, breaking through the limitations of traditional forward-looking sonar SLAM technology in feature-rich environments. Since the present invention does not rely on the feature richness of the environment, it is applicable to both feature-rich areas such as ports and feature-sparse environments such as the deep sea. This strong adaptability enables the present invention to play an important role in a variety of marine exploration scenarios, and at the same time provides a new solution for deep-sea exploration far from the shore with sparse features.
[0029] In addition, in view of the common noise problem in forward-looking sonar images, the present invention can effectively reduce the impact of noise on the accuracy of 3D reconstruction by constructing and applying constraint models (plane constraints and spherical constraints). Compared with the method of 3D reconstruction that relies solely on sonar images, the present invention can provide more stable and accurate results while improving the processing speed.
[0030] In addition, the present invention integrates the existing forward-looking sonar technology and underwater submersible navigation data, and does not require the introduction of additional equipment or technology, so it is relatively simple in actual operation. For operators, this means lower learning costs and higher work efficiency. The present invention has important practical value. BRIEF DESCRIPTION OF THE DRAWINGS
[0031] 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 only embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on the provided drawings without paying creative work.
[0032] Figure 1 A spatial schematic diagram of sonar positioning provided by the present invention.
[0033] Figure 2 A schematic diagram of the pixel correspondence between the polar coordinate system fan-shaped image and the original forward-looking sonar image provided by the present invention.
[0034] Figure 3 This is a flow chart of the target three-dimensional positioning method provided by the present invention. DETAILED DESCRIPTION
[0035] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions 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. 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.
[0036] In the actual use of underwater submersibles, in order to complete a wide range of underwater tasks, they are usually equipped with a high-precision underwater navigation system consisting of an inertial navigation unit (INS) and an underwater acoustic velocimeter (DVL). This navigation system can provide accurate status data and positioning data for underwater submersibles, significantly increasing their operating range, and therefore has been widely used in underwater submersibles.
[0037] Compared with the positioning results of forward-looking sonar SLAM, the positioning accuracy of the underwater navigation system is higher and is not affected by ocean features. Therefore, forward-looking sonar target positioning combined with navigation system status data can provide more stable results and will not affect the normal navigation of underwater submersibles in open waters.
[0038] Therefore, in response to the actual demand for three-dimensional positioning of seabed targets in ocean exploration missions, the present invention provides a high-precision three-dimensional positioning method for forward-looking sonar targets in combination with underwater submersible navigation data. By combining the forward-looking sonar image target recognition and registration results, and the attitude and positioning data output by the underwater submersible navigation system, high-precision three-dimensional positioning of seabed targets is achieved. Specifically,
[0039] In one embodiment, a target three-dimensional positioning method applied to an underwater vehicle comprises the following steps:
[0040] Identify the target in the sonar image, and then determine the surface constraint of the target based on the position coordinates of the underwater vehicle corresponding to the current target, wherein the surface constraint includes a plane constraint and a spherical constraint;
[0041] Based on the surface constraint at the first moment and the surface constraint at the second moment, a constraint model is constructed, the constraint model is solved, and the target is positioned in three dimensions. Preferably, the time interval between the first moment and the second moment is relatively large, because the change in the observation angle of the same object at two consecutive moments is very small, which will lead to a very poor positioning effect, and it is necessary to select two observations with a long interval time to locate the same object. In one embodiment, a threshold value between the two moments can be set to constrain it.
[0042] This application uses plane constraints and spherical constraints to establish a constraint model and obtains the target positioning result by solving it. The plane constraint is the constraint on the beam surface where the target is located.
[0043] In this embodiment, the normal vector V of the beam surface where the target is located kn =[Vx kn ,Vy kn ,Vz kn ] T In the world coordinate system, it is expressed as:
[0044]
[0045] In the formula, R k is the rotation matrix of the sonar coordinate system relative to the world coordinate system, which can be provided by the navigation system of the underwater vehicle, θ kn It is the azimuth of the target in the sonar coordinate system, which can be obtained through forward-looking sonar target recognition.
[0046] Furthermore, the world coordinates of the object are defined as the target position According to the point-by-point plane calculation method, the beam surface where the object is located can be expressed as:
[0047]
[0048] The beam surface is represented as of the form, where A kn , B kn , C kn and D kn represents the coefficients of the plane equation; and
[0049]
[0050] (X k , Y k , Z k ) is the position coordinate of the underwater vehicle at time k.
[0051] For spherical constraints, when the distance r between the target and the sonar is obtained through forward-looking sonar image target recognition kn After that, the distance constraint equation can be established according to the forward-looking sonar imaging process. Its image in three-dimensional space is described as a sphere. The equation of the spherical constraint is as follows:
[0052]
[0053] In order to better integrate the plane constraint and the spherical constraint, this embodiment solves the three observation equations into an equation system, that is, the constraint model includes the plane constraint at the first moment, the plane constraint at the second moment, and the spherical constraint at the first moment, or the plane constraint at the first moment, the plane constraint at the second moment, and the spherical constraint at the second moment.
[0054] In one embodiment, the forward-looking sonar observation results at time j and time k are randomly obtained from the data that can observe the target n, and two plane constraint methods and two spherical constraint equations are established at the same time. Then, the two plane constraint equations are combined with the two spherical constraint equations to form the following model expressions:
[0055]
[0056] or
[0057]
[0058] (X j , Y j , Z j ) is the position coordinate of the underwater vehicle at time j.
[0059] The spatial description and analysis of plane constraints and spherical constraints are as follows: Figure 1 As shown. From the spatial analysis of forward-looking sonar imaging, it is found that the plane intersection line and the spatial sphere have two intersection points, corresponding to the two solutions in the equation. Since the plane intersection line is vertical, the solution with the larger z coordinate (target depth) of the two solutions of the equation group is taken as the correct target position, as shown in Figure 1 middle location.
[0060] In the existing positioning method, the sonar raw data is generally converted into a fan-shaped image to facilitate the recognition algorithm to perform recognition, and then the recognition result is converted into the azimuth and slant range of the target according to the sonar imaging principle to calculate the positioning result of the target. The steps include:
[0061] First, the original polar coordinate system image of the forward-looking sonar needs to be converted into a Cartesian coordinate system fan diagram that is easier for people to understand, so as to facilitate subsequent target identification. The coordinates of the target in the polar coordinate system sonar image are:
[0062]
[0063] Where (x, y) is the coordinate of the target in the sonar image in the polar coordinate system, Y is the number of beam planes emitted by the forward-looking sonar, D is the detection range set by the forward-looking sonar, X is the number of sampling points of the forward-looking sonar in a single beam plane, α is the beam opening angle of the forward-looking sonar, and r is kn is the distance between the target and the origin of the sonar coordinate system, θ kn It is the azimuth from the target point to the sonar XZ plane. These parameters are determined by the forward-looking sonar device itself.
[0064] Since the polar coordinate image of the forward-looking sonar is not easy to understand, the polar coordinate image of the forward-looking sonar needs to be projected into the Cartesian coordinate system to form a fan-shaped diagram. The projection correspondence from the polar coordinate image to the Cartesian coordinate image is as follows: Figure 2 The specific projection relationship is as follows:
[0065]
[0066] Then, the coordinates of the target in the sonar image are converted to the sonar coordinate system according to the following formula;
[0067]
[0068] is the coordinate of target n in the three-dimensional space at time k in the sonar coordinate system; r kn is the distance between the target and the origin of the sonar coordinate system. kn is the azimuth from the target to the sonar XZ plane, φ kn is the elevation angle of the target point in the sonar coordinate system;
[0069] Finally, the target coordinates in the sonar coordinate system are converted to the world coordinate system as follows;
[0070]
[0071] Among them, R k , T k is the rotation matrix and displacement vector of the sonar coordinate system relative to the world coordinate system at time k, which can be provided by the navigation system of the underwater vehicle; are the coordinates of the same target in the world coordinate system.
[0072] It should be noted that after forward-looking sonar target recognition and registration based on deep learning methods, although the azimuth angle θ of the target in the sonar coordinate system can be determined kn and slope distance r kn , but in the forward-looking sonar imaging process, the object's elevation angle φ kn There will be a lack of elevation angle, that is, the elevation angle cannot be obtained from the sonar image. Accordingly, the target positioning method provided by the present invention can bypass the elevation angle and directly calculate the three-dimensional coordinates of the object.
[0073] To further optimize the above technical solution, the underwater vehicle is equipped with a forward-looking sonar and a navigation system. The forward-looking sonar is used to scan and obtain sonar images, and the navigation system is used to provide the position coordinates and attitude information of the underwater vehicle at different times.
[0074] Traditional optical image registration mostly uses image registration methods based on feature points. However, compared with optical images, forward-looking sonar images have problems such as large noise and low image resolution. Image registration methods based on feature points often cannot achieve good results in forward-looking sonar images. Therefore, the present invention uses a forward-looking sonar image target recognition method based on deep learning as a method for identifying and registering targets in continuous sonar images, and realizes semantic-level registration of targets in forward-looking sonar images.
[0075] When positioning, Figure 3 As shown, firstly, the navigation system and forward-looking sonar of the underwater vehicle are started, so that the underwater vehicle starts the scanning task. When the underwater vehicle automatically navigates in the ocean, it needs to keep its own attitude stable, keep sailing in a straight line during most of the scanning process, and its own depth is relatively stable, while performing Z-shaped automatic scanning;
[0076] The forward-looking sonar data is further read and converted into a fan-shaped image. When a target appears in the forward-looking sonar image, the target recognition method of deep learning is used to realize target recognition, positioning and registration in continuous forward-looking sonar images. At the same time, data with poor recognition and registration results are filtered out. Then, the registration results of the targets in the continuous sonar images and the corresponding underwater submersible attitude data and positioning data are read; and the multiple target registration results and navigation data are saved.
[0077] In this application, when a target is found in a forward-looking sonar image by a target recognition method based on deep learning, the position of the target in the forward-looking sonar image is continuously recorded, and the attitude information and position coordinates of the underwater submersible output by the underwater submersible navigation system at the corresponding time are read, wherein the attitude information includes the rotation matrix R of the sonar coordinate system relative to the world coordinate system. k and displacement vector T k .
[0078] Furthermore, the position of the target in the image is saved together with the navigation data of the underwater vehicle to form the target observation data. When the target recognition algorithm detects that the target disappears from the forward-looking sonar image, it starts to calculate the positioning result of the target.
[0079] Then, the observation data at any two different times are randomly selected to perform multiple three-dimensional positioning, and the average of multiple positioning results is used as the target three-dimensional positioning result.
[0080] Specifically:
[0081] Randomly select two image registration results and corresponding navigation data;
[0082] The solution equation is established by combining the navigation data and the target positioning results of the forward-looking sonar image;
[0083] Solve the plane intersection line using data and plane constraint equations;
[0084] The target positioning is solved using the plane intersection equation and directional distance constraints.
[0085] When x positioning results can be calculated, the multiple target positioning results are averaged to obtain a high-precision positioning result of the target; if x positioning results cannot be obtained, the image registration result and the corresponding navigation data are returned to be re-selected for three-dimensional positioning.
[0086] After multiple rounds of random data extraction and solving for multiple target positions, all target positions are averaged to eliminate the noise error of the observation and obtain the final target positioning result.
[0087] In this example, in order to verify the good effect of the present invention, the collected sonar data and underwater submersible navigation data are subjected to comparative experiments using the plane constraint method, the spherical constraint method, the plane constraint plus spherical constraint method and the present invention respectively. At the same time, the seabed height under the robot collected by DVL is used as a reference value to compare the positioning effects of different methods. The comparative test results are shown in the following table;
[0088]
[0089]
[0090] As can be seen from the table, the positioning results based on plane constraints and the method proposed in this paper are more reasonable. The depth of the object is also closer to the reference value of the seabed depth. At the same time, the solution speed of the method proposed in this paper is much faster than that of the method based on plane constraints. Therefore, the method proposed in this paper is more excellent in the task of real underwater submersibles carrying forward-looking sonars to locate targets.
[0091] In this specification, each embodiment is described in a progressive manner, and each embodiment focuses on the differences from other embodiments. The same or similar parts between the embodiments can be referred to each other. For the device disclosed in the embodiment, since it corresponds to the method disclosed in the embodiment, the description is relatively simple, and the relevant parts can be referred to the method part.
[0092] The above description of the disclosed embodiments enables one skilled in the art to implement or use the present invention. Various modifications to these embodiments will be apparent to one skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of the present invention. Therefore, the present invention will not be limited to the embodiments shown herein, but rather to the widest scope consistent with the principles and novel features disclosed herein.
Claims
1. A three-dimensional positioning method for underwater vehicles. It is characterized in that After identifying the target in the sonar image, the surface constraints of the target are determined based on the position coordinates of the underwater vehicle corresponding to the current target. Based on the surface constraint at the first moment and the surface constraint at the second moment, a constraint model is constructed, the constraint model is solved, and the target is three-dimensionally positioned.
2. A method for three-dimensional positioning of a target applied to an underwater vehicle according to claim 1, It is characterized in that The time interval between the first moment and the second moment is greater than a set threshold.
3. The method for three-dimensional positioning of a target applied to an underwater vehicle according to claim 1, It is characterized in that The surface constraint includes a plane constraint and a spherical constraint; the constraint model includes a plane constraint at a first moment, a plane constraint at a second moment, and a spherical constraint at the first moment, or a plane constraint at the first moment, a plane constraint at a second moment, and a spherical constraint at the second moment.
4. The method for three-dimensional positioning of a target applied to an underwater vehicle according to claim 2, It is characterized in that When solving the constraint model, the solution corresponding to the larger z coordinate among the obtained solutions is taken as the target position.
5. The method for three-dimensional positioning of a target applied to an underwater vehicle according to claim 2, It is characterized in that The expression for the plane constraint is: A kn X+B kn Y+C kn Z+D kn =0 In the formula, A kn , B kn , C kn and D kn represents the coefficients of the plane equation, where In the formula, [Vx kn ,Vy kn ,Vz kn ] T is the normal vector of the plane, (X k , Y k , Z k ) is the position coordinate of the underwater vehicle at time k; and In the formula, R k is the rotation matrix of the sonar coordinate system relative to the world coordinate system, θ kn is the azimuth angle θ of the target in the sonar coordinate system kn .
6. The method for three-dimensional positioning of a target applied to an underwater vehicle according to claim 1, It is characterized in that Identify the target in the sonar image and record the underwater vehicle position information corresponding to the target at the current moment to form target observation data; Randomly extract observation data at any two different times, perform multiple three-dimensional positioning, and take the average of multiple positioning results as the target three-dimensional positioning result.
7. The method for three-dimensional positioning of a target applied to an underwater vehicle according to claim 1, It is characterized in that The underwater vehicle is equipped with a forward-looking sonar and a navigation system. The forward-looking sonar is used to scan and obtain sonar images and identify targets in the sonar images. The navigation system is used to provide the position coordinates of the underwater vehicle at different times.
Citation Information
Cited By
Anti-drowning early warning method and system for robot, robot and medium
CN122336917A
