Underwater target detection and positioning method based on data intervention of sonar images

Abstract

The present invention discloses a method for detecting and locating underwater targets using data intervention based on sonar images. By constructing a three-dimensional rectangular coordinate system, the azimuth of the underwater target is calculated based on the output of a deep learning network. The target is then located by querying the echo time based on the azimuth. Depending on the distance of the target, an underwater robot is controlled to grasp it or continue moving by compensating for the forward angle based on the calculated deviation rate. While ensuring accurate positioning, the underwater robot reduces the number of underwater target positioning attempts and enables rapid approach to the target. During the grasping process, the target's positioning coordinates are optimized, improving grasping accuracy.

Description

Technical Field

[0001] The present invention relates to the field of underwater target detection and positioning, and in particular to a method for underwater target detection and positioning based on sonar image data intervention. Background Art

[0002] Underwater target detection and positioning is one of the key technologies for underwater autonomous operation robots and an important way for them to obtain information about unknown underwater environments. Reliable target detection and positioning methods can ensure that underwater robots can effectively complete underwater operations.

[0003] Common methods for target detection and localization based on sonar images can be roughly categorized into two main categories: those based on traditional image processing algorithms and those based on machine learning. Current technologies can detect and locate underwater targets, but when underwater robots approach distant targets, the underwater environment can cause them to deviate from their intended paths, making them inefficient in underwater missions. While real-time sonar scanning can accomplish target tracking, it consumes too much energy for underwater robots, making them unsuitable for extended periods of operation. Summary of the Invention

[0004] In response to the shortcomings of the existing technology, the present invention provides a method for underwater target detection and positioning based on sonar image data intervention to solve the technical problems in the existing technology that underwater robot target tracking tasks consume too much energy, have low tracking accuracy, and are not conducive to long-term work.

[0005] The present invention provides a method for detecting and locating underwater targets based on sonar image data intervention, comprising the following steps:

[0006] Step 1: When the underwater robot begins to search for underwater targets, a three-dimensional rectangular coordinate system is established with the underwater robot's sonar imaging device as the origin;

[0007] Step 2: Obtain a sonar image through a sonar imaging device, use the sonar image as the input of the deep learning network, calculate the azimuth of the underwater target based on the output of the deep learning network, and query the echo time based on the azimuth;

[0008] Step 3: Obtain the current detection angle of the sonar imaging device and obtain the positioning coordinates of the underwater target;

[0009] When the positioning coordinates of the underwater target object are within the working range of the underwater robot actuator, step A4 is executed;

[0010] When the positioning coordinates of the underwater target object are not within the operating range of the underwater robot's actuator, the underwater robot is driven forward at a preset driving speed, a preset driving time, and a detection angle α as a forward angle. After the preset driving time expires, steps B4-B6 are executed.

[0011] Step A4: Reestablishing a three-dimensional coordinate system with the sonar imaging device as the origin, controlling the actuator of the underwater robot to move toward the underwater target, acquiring the coordinates of the actuator and the underwater target in real time, calculating positioning correction parameters based on the actuator coordinates, correcting the underwater target coordinates using the positioning correction parameters, and controlling the actuator to move toward the corrected underwater target coordinates until the actuator moves to a position where it can grasp the underwater target, thereby grasping the underwater target.

[0012] Step B4: obtaining the current position coordinates of the underwater robot, calculating the ideal position coordinates of the underwater robot, and calculating the movement deviation rate according to the current position coordinates of the underwater robot and the ideal position coordinates of the underwater robot;

[0013] Step B5: Calculating the actual movement deviation rate based on the movement deviation rate obtained in step B4 and the historical movement deviation rate, and compensating the forward angle of the underwater robot based on the actual movement deviation rate;

[0014] Step B6: driving the underwater robot forward at a preset driving speed, a preset driving time, and a compensated forward angle, and returning to step 2 after the preset driving time is over.

[0015] Furthermore, the calculation formula of the azimuth angle is as follows:

[0016]

[0017] Among them, (o x , o y ) is the coordinate of the center point of the prediction box, (s x , s y ) are the coordinates of the sonar device in the image.

[0018] Furthermore, a specific method for obtaining the current detection angle of the sonar imaging device is: the angle between the plane where the current sonar device imaging range is located and the xoz plane in the three-dimensional coordinate system.

[0019] Furthermore, the calculation formula for obtaining the positioning coordinates of the underwater target is as follows:

[0020]

[0021] Among them, τ is the echo time, C is the underwater sound wave speed, α is the detection angle of the sonar equipment, and β is the azimuth.

[0022] Furthermore, in step B4, the formula for obtaining the current position coordinates of the underwater robot is as follows:

[0023]

[0024] Among them, (x0y0) is the initial target positioning result, (xi ,y i ) is the current position target positioning result, δ i is the current forward angle of the underwater robot.

[0025] The formula for obtaining the ideal position coordinates of the underwater robot is as follows:

[0026]

[0027] Among them, (x p′i-1 ,y p′i-1 ) is the actual coordinate of the underwater robot during the last detection, V is the preset driving speed, T is the preset driving time, δ i is the current forward angle of the underwater robot.

[0028] The calculation formula of moving deviation rate is as follows:

[0029]

[0030] Among them, (x p′i ,y p′i ) is the current position coordinate of the underwater robot, (x p′i-1 ,y p′i-1 ) is the actual coordinate of the underwater robot during the last detection, (x pi ,y pi ) is the ideal position coordinate of the underwater robot.

[0031] Furthermore, the calculation formula for calculating the actual movement deviation rate based on the movement deviation rate obtained in step B4 and the historical movement deviation rate is as follows:

[0032] σ i =(σ xi , σ yi )=σ i-1 +Δ i

[0033] Among them, σ i-1 is the historical moving deviation rate, Δ i is the movement deviation rate obtained in step B4.

[0034] Furthermore, the formula for obtaining the forward angle after compensation in step B6 is as follows:

[0035]

[0036] Among them, (x0, y0) is the initial target positioning result, (x p′i ,y p′i ) is the current position coordinate of the underwater robot, (σ xi , σ yi ) is the actual moving deviation rate.

[0037] Furthermore, the calculation formula of the positioning correction parameter is as follows:

[0038]

[0039] Among them, u, v, w are the coordinates of the actuator obtained by sonar positioning; u′, v, w′ are the coordinates of the actuator obtained by the underwater robot itself.

[0040] Beneficial effects of the present invention:

[0041] The present invention aims to detect and locate underwater targets. It uses sonar equipment to collect underwater environmental information, and locates underwater targets through the information collected by the sonar equipment and the output data of the deep network. For targets outside the working range of the actuator, the actuator locates them multiple times during the approach process, and calculates the movement deviation rate based on the positioning results to compensate the forward angle of the underwater robot, so that the underwater robot can quickly approach the underwater target. For targets within the working range of the actuator, the actuator and the underwater target are located, and the underwater target positioning result is corrected according to the accurate coordinates of the actuator, providing accurate and reliable three-dimensional positioning information of the underwater target for underwater robot operations. BRIEF DESCRIPTION OF THE DRAWINGS

[0042] The features and advantages of the present invention will be more clearly understood by referring to the accompanying drawings, which are schematic and should not be construed as limiting the present invention in any way. In the accompanying drawings:

[0043] Figure 1 is a flow chart of a specific embodiment of the present invention;

[0044] Figure 2 It is a modeling schematic diagram of a specific embodiment of the present invention;

[0045] Figure 3 1. D-Ynet structure diagram and YoloHead structure diagram in a specific embodiment of the present invention;

[0046] Figure 4 is the D-Ynet training result in a specific embodiment of the present invention;

[0047] Figure 5 It is a schematic diagram of a method initially defined in a specific embodiment of the present invention;

[0048] Figure 6 is a schematic diagram of a process of approaching an underwater target object in a specific embodiment of the present invention;

[0049] Figure 7 3 is a comparison diagram of positioning errors before and after correction of a specific embodiment of the present invention. DETAILED DESCRIPTION

[0050] To make the purpose, technical solutions, and advantages of the embodiments of the present invention more clear, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the accompanying 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. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without making any creative efforts shall fall within the scope of protection of the present invention.

[0051] The present invention will be further described below with reference to specific examples. Those skilled in the art will appreciate that these examples are intended only to illustrate the present invention and are not intended to limit the scope of the present invention, and that modifications to various equivalent forms of the present invention fall within the scope defined by the appended claims.

[0052] like Figure 1 As shown, the present invention provides a method for detecting and locating underwater targets based on sonar image data intervention, comprising the following steps:

[0053] Step 1: When the underwater robot begins to search for underwater targets, a three-dimensional rectangular coordinate system is established with the underwater robot's sonar imaging device as the origin;

[0054] Establish a three-dimensional rectangular coordinate system {0} such as Figure 2 As shown, the sonar device and the underwater manipulator device are fixed on the bottom of the underwater robot at a distance of S meters. With the sonar device as the origin, the direction along the installation plane pointing to the head of the robot is the positive direction of the x-axis, and the direction perpendicular to the installation plane downward is the positive direction of the z-axis.

[0055] Step 2: Obtain a sonar image through a sonar imaging device, use the sonar image as the input of the deep learning network, calculate the azimuth of the underwater target based on the output of the deep learning network, and query the echo time based on the azimuth;

[0056] The deep learning network is specifically the D-Ynet detection network, and its structure is as follows Figure 3 As shown in the figure, after batch normalization is activated, image features are extracted through 11 residual blocks, and the features are stacked and input into the YoloHead detection head. The non-maximum suppression method is used for all predicted target position coordinates and confidence results, and the prediction results Obj, Anc and Cls with the highest confidence are output; where Obj indicates whether there is a target object in the current image, Anc indicates the coordinates of the center point of the prediction box (o x , o y ), Cls represents the type of target; the D-Ynet detection network must be trained before use, specifically: the existing sonar images containing underwater targets and actuators are divided into training sets and test sets without intersections and sent to the detection network for training and testing to identify targets and actuators in the sonar images; the D-Ynet detection network training results are as follows Figure 4As shown, the network can be trained to detect actuators and targets in sonar images;

[0057] The azimuth angle is calculated as follows:

[0058]

[0059] Among them, (o x , o y ) is the coordinate of the center point of the prediction box, (s x , s y ) are the coordinates of the sonar device in the image.

[0060] Query the echo time based on the azimuth angle β. To avoid the error between the estimated β value and the true value, which may result in the inability to find the echo time corresponding to the angle, an angle error compensation ε is set, and the minimum echo time corresponding to the interval (β-ε, β+ε) is recorded as τ.

[0061] Step 3: Obtain the current detection angle of the sonar imaging device and obtain the positioning coordinates of the underwater target;

[0062] When the positioning coordinates of the underwater target are within the operating range of the underwater robot actuator, execute step A4:

[0063] When the positioning coordinates of the underwater target object are not within the working range of the underwater robot's actuator, the underwater robot is driven forward at a preset driving speed, a preset driving time, and a detection angle α as the forward angle. After the preset driving time is over, steps B4-B6 are executed; Figure 6 As shown;

[0064] The detection angle of current sonar imaging equipment is as follows Figure 2 As shown, it is the angle between the plane where the current sonar equipment imaging range is located and the xoz plane in the three-dimensional coordinate system, obtained by the underwater robot, and recorded as α;

[0065] The calculation formula for obtaining the positioning coordinates of underwater targets is as follows: Figure 5 As shown:

[0066]

[0067] Among them, τ is the echo time, C is the underwater sound wave speed, α is the detection angle of the sonar equipment, and β is the azimuth.

[0068] Step A4: Reestablishing a three-dimensional coordinate system with the sonar imaging device as the origin, controlling the actuator of the underwater robot to move toward the underwater target, acquiring the coordinates of the actuator and the underwater target in real time, calculating positioning correction parameters based on the actuator coordinates, correcting the underwater target coordinates using the positioning correction parameters, and controlling the actuator to move toward the corrected underwater target coordinates until the actuator moves to a position where it can grasp the underwater target, thereby grasping the underwater target.

[0069] The method of establishing the three-dimensional coordinate system {0} is the same as step 1. The positioning method of the actuator and the underwater target is the same as steps 2 and 3. The actuator coordinate S is obtained. a = (u, v, w) and the underwater target coordinate S1; the exact coordinates of the actuator are obtained by the underwater robot itself, denoted as

[0070] according to With S a Positioning correction parameters can be obtained

[0071]

[0072] Correct the coordinates of the underwater target to obtain the final positioning result.

[0073]

[0074] Through simulation experiments, the positioning results of underwater targets within 10m are corrected. The errors before and after correction are as follows: Figure 7 As shown in the figure, the feasibility and effectiveness of this method are demonstrated.

[0075] Step B4: obtaining the current position coordinates of the underwater robot, calculating the ideal position coordinates of the underwater robot, and calculating the movement deviation rate according to the current position coordinates of the underwater robot and the ideal position coordinates of the underwater robot;

[0076] According to the coordinate transformation law, there is a relationship between the current position target positioning result and the initial target positioning result:

[0077]

[0078] The formula for obtaining the current position coordinates of the underwater robot is as follows:

[0079]

[0080] Among them, (x0, y0) is the initial target positioning result, (x i ,y i ) is the current position target positioning result, δ i is the current forward angle of the underwater robot.

[0081] If there is no underwater environment interference, the formula for obtaining the ideal position coordinates of the underwater robot is as follows:

[0082]

[0083] Among them, (x p′i-1 ,y p′i-1 ) is the actual coordinate of the underwater robot during the last detection, V is the preset driving speed, T is the preset driving time, δ i is the current forward angle of the underwater robot.

[0084] The calculation formula of moving deviation rate is as follows:

[0085]

[0086] Among them, (x p′i ,y p′i ) is the current position coordinate of the underwater robot, (x p′i-1 ,y p′i-1 ) is the actual coordinate of the underwater robot during the last detection, (x pi ,y pi ) is the ideal position coordinate of the underwater robot.

[0087] Step B5: Calculating the actual movement deviation rate based on the movement deviation rate obtained in step B4 and the historical movement deviation rate, and compensating the forward angle of the underwater robot based on the actual movement deviation rate;

[0088] After using the historical deviation rate for compensation, there is still a deviation Δ in the movement process i , then the calculation formula of the actual moving deviation rate is as follows:

[0089] σ i =(σ xi , σ yi )=σ i-1 +Δ i

[0090] Step B6: driving the underwater robot forward at a preset driving speed, a preset driving time, and a compensated forward angle, and returning to step 2 after the preset driving time is over.

[0091] The preset speed and time are:

[0092]

[0093] Among them, V max is the preset maximum driving speed, T max is the preset maximum travel time, and L is the maximum arm span of the underwater manipulator;

[0094] The formula for obtaining the forward angle after compensation is as follows:

[0095]

[0096] Among them, (x0, y0) is the initial target positioning result, (x p′i ,y p′i ) is the current position coordinate of the underwater robot, (σ xi , σ yi ) is the actual moving deviation rate.

[0097] Although the embodiments of the present invention have been described with reference to the accompanying drawings, those skilled in the art may make various modifications and variations without departing from the spirit and scope of the present invention. Such modifications and variations are all within the scope defined by the appended claims.

Claims

1. A method for underwater target detection and positioning based on sonar image data intervention, characterized in that: The steps include: Step 1: When the underwater robot begins to search for underwater targets, a three-dimensional rectangular coordinate system is established with the underwater robot's sonar imaging device as the origin; Step 2: Obtain a sonar image through a sonar imaging device, use the sonar image as the input of the deep learning network, calculate the azimuth of the underwater target based on the output of the deep learning network, and query the echo time based on the azimuth; Step 3: Obtain the current detection angle of the sonar imaging device and obtain the positioning coordinates of the underwater target; When the positioning coordinates of the underwater target object are within the working range of the underwater robot actuator, step A4 is executed; When the positioning coordinates of the underwater target object are not within the operating range of the underwater robot's actuator, the underwater robot is driven forward at a preset driving speed, a preset driving time, and the detection angle is set as the forward angle. After the preset driving time expires, steps B4-B6 are executed; Step A4: Reestablishing a three-dimensional coordinate system with the sonar imaging device as the origin, controlling the actuator of the underwater robot to move toward the underwater target, acquiring the coordinates of the actuator and the underwater target in real time, calculating positioning correction parameters based on the actuator coordinates, correcting the underwater target coordinates using the positioning correction parameters, and controlling the actuator to move toward the corrected underwater target coordinates until the actuator moves to a position where it can grasp the underwater target, thereby grasping the underwater target. Step B4: obtaining the current position coordinates of the underwater robot, calculating the ideal position coordinates of the underwater robot, and calculating the movement deviation rate according to the current position coordinates of the underwater robot and the ideal position coordinates of the underwater robot; Step B5: Calculating the actual movement deviation rate based on the movement deviation rate obtained in step B4 and the historical movement deviation rate, and compensating the forward angle of the underwater robot based on the actual movement deviation rate; Step B6: driving the underwater robot forward at a preset driving speed, a preset driving time, and a compensated forward angle, and returning to step 2 after the preset driving time is over.

2. The underwater target detection and positioning method based on sonar image data intervention according to claim 1, characterized in that: The azimuth angle is calculated as follows: Among them, (o x , o y ) is the coordinate of the center point of the prediction box, (s x , s y ) are the coordinates of the sonar device in the image.

3. The underwater target detection and positioning method based on sonar image data intervention according to claim 1, characterized in that: The specific method for obtaining the detection angle of the current sonar imaging device is: the angle between the plane where the current sonar device imaging range is located and the xoz plane in the three-dimensional coordinate system.

4. The underwater target detection and positioning method based on sonar image data intervention according to claim 2, characterized in that: The calculation formula for obtaining the positioning coordinates of underwater targets is as follows: Among them, τ is the echo time, C is the underwater sound wave speed, α is the detection angle of the sonar equipment, and β is the azimuth.

5. The underwater target detection and positioning method based on sonar image data intervention according to claim 4, characterized in that: The formula for obtaining the current position coordinates of the underwater robot in step B4 is as follows: Among them, (x0, y0) is the initial target positioning result, (x i ,y i ) is the current position target positioning result, δ i is the current forward angle of the underwater robot; The formula for obtaining the ideal position coordinates of the underwater robot is as follows: Among them, (x p′i-1 ,y p′i-1 ) is the actual coordinate of the underwater robot during the last detection, V is the preset driving speed, T is the preset driving time, δ i is the current forward angle of the underwater robot; The calculation formula of moving deviation rate is as follows: Among them, (x p′i ,y p′i ) is the current position coordinate of the underwater robot, (x p′i-1 ,y p′i-1 ) is the actual coordinate of the underwater robot during the last detection, (x pi ,y pi ) is the ideal position coordinate of the underwater robot.

6. The underwater target detection and positioning method based on sonar image data intervention according to claim 5, characterized in that: The formula for calculating the actual movement deviation rate based on the movement deviation rate obtained in step B4 and the historical movement deviation rate is as follows: s i =(s xi ,s yi )=s i-1 +D i Among them, σ i -1 is the historical moving deviation rate, Δ i is the movement deviation rate obtained in step B4.

7. The underwater target detection and positioning method based on sonar image data intervention according to claim 6, characterized in that: The formula for obtaining the forward angle after compensation in step B6 is as follows: Among them, (x o ,y o ) is the initial target positioning result, (x p′i ,y p′i ) is the current position coordinate of the underwater robot, (σ xi ,σ yi ) is the actual moving deviation rate.

8. The underwater target detection and positioning method based on sonar image data intervention according to claim 7, characterized in that: The calculation formula for positioning correction parameters is as follows: Among them, u, v, w are the coordinates of the actuator obtained by sonar positioning; u′, v′, w′ are the coordinates of the actuator obtained by the underwater robot itself.

Images