A seabed inclination detection method based on three-dimensional imaging sonar point cloud coordinates
By fitting a plane with 3D imaging sonar point cloud data and combining it with inertial navigation and attitude sensor information, the problem of low efficiency and large error in seabed tilt detection in existing technologies has been solved, achieving high-precision, real-time seabed tilt detection.
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- SUZHOU SOUNDTECH OCEANIC INSTR
- Filing Date
- 2024-01-22
- Publication Date
- 2026-06-30
Smart Images
Figure CN117849809B_ABST
Abstract
Description
Technical Field
[0001] This invention relates to the technical field of underwater tilt detection, and more specifically, to a method for detecting seabed tilt based on three-dimensional imaging sonar point cloud coordinates. Background Technology
[0002] Three-dimensional imaging sonar uses a two-dimensional planar transducer array as a receiver and employs a pre-formed multi-beam method to perform imaging through beamforming. It can simultaneously obtain target information in the horizontal, vertical, and distance directions and present underwater three-dimensional targets in real time in the form of three-dimensional point clouds. It can reflect the underwater three-dimensional scene in a three-dimensional and intuitive way and plays an important role in underwater engineering fields such as hydrological measurement, dam construction, and seabed exploration.
[0003] During operation, the 3D imaging sonar periodically emits acoustic pulse signals from the transmitting transducer. After being reflected by obstacles, the signals reach the receiving transducer array. After acoustic-to-electric conversion, the signal processor performs beamforming on the channel echo signals. The signals from different transducers are superimposed with corresponding time delays or phase shifts to form spatial beams in two dimensions. The energy of the beam signals in each direction is extracted and filtered to obtain a 3D point cloud image. Each voxel in the image contains its 3D coordinates and intensity information. Moreover, the sonar itself carries an attitude sensor that can return the sonar's attitude information in real time.
[0004] The most common method for measuring the bottom tilt angle is to use a single-beam depth sounder to repeatedly measure the depth of the seabed during the horizontal navigation of the vehicle, and infer the tilt angle by observing the changes in depth. However, this method can only measure the tilt angle of a local area of the seabed along the route of the vehicle, which will have a large deviation compared with the tilt angle of the actual seabed scene. Moreover, it is inefficient to obtain tilt angle data only after multiple depth measurements. Summary of the Invention
[0005] The purpose of this invention is to provide a method for detecting seabed tilt angle that combines the characteristics of three-dimensional imaging sonar data.
[0006] The technical solution of this invention is: to provide a method for detecting seabed tilt angle based on the coordinates of a three-dimensional imaging sonar point cloud. This method is applied to a three-dimensional imaging sonar mounted on an underwater device, and includes:
[0007] S1. Fit multiple planes based on the three-dimensional point cloud data generated by the three-dimensional imaging sonar, and select the best plane as the output plane based on the distribution of the three-dimensional point cloud.
[0008] S2. Calculate the tilt angle of the output plane relative to the sonar coordinate system, and then perform angle correction based on the tilt angle of the sonar in the vertical plane to solve the tilt angle of the output plane relative to the geodetic coordinate system.
[0009] Among them, in step S1, after fitting each plane, calculate the shortest distance between all the three-dimensional point clouds and the fitted plane. All the three-dimensional point clouds with a distance less than the distance threshold r are set as the inliers of the fitted plane;
[0010] Divide the fitted plane into grids of a fixed size, project all the inliers onto the fitted plane, screen all the grids containing the projections of the inliers, calculate the total area of the screened grids as the inlier projection area record, and select the fitted plane with the largest inlier projection area as the best plane.
[0011] In any of the above technical solutions, further, step S1 includes:
[0012] S11. Establish a right-handed three-dimensional coordinate system in space with the three-dimensional imaging sonar as the origin as the sonar coordinate system. The orientation of the three-dimensional imaging sonar is used as the positive direction of the z-axis, the vertical plane where the z-axis is located is used as the yz plane, and the positive direction of the y-axis is upward;
[0013] S12. Randomly select three points P1(x1, y1, z1), P2(x2, y2, z2), and P3(x3, y3, z3) in the three-dimensional point cloud data, and calculate the plane equation fitted by these three points: ax + by + z + c = 0, where a, b, and c are parameters;
[0014] S13. Set the distance threshold r, and divide all the three-dimensional point clouds into inliers and outliers according to the set distance threshold r. Among them, the inliers represent the set of points near the fitted plane: S in = {(x, y, z)||ax + by + z + c| < r}; the outliers represent the set of points far from the fitted plane: S out = {(x, y, z)||ax + by + z + c| > r};
[0015] S14. Divide the fitted plane into grids of a fixed size, project all the inliers onto the fitted plane, screen all the grids containing the projections of the inliers, and calculate the total area of the screened grids as the inlier projection area record;
[0016] S15. Repeat steps S12 to S14 for a preset number of times. After execution, select the fitted plane with the largest recorded inlier projection area;
[0017] [[ID=2,8]]S16. Perform least squares fitting on the inliers of the fitted plane selected in step S15, and use the refined fitted plane as the output plane.
[0018] In any of the above technical solutions, further, step S2 specifically includes:
[0019] In the sonar coordinate system, by projecting the normal line of the output plane onto the YZ plane, the pitch angle can be obtained planeSimilarly, by projecting the plane normal onto the XY plane, the yaw angle Roll can be obtained. plane ;
[0020] The calculated tilt angle is corrected for geodetic coordinates and combined with the sonar pitch angle output from the attitude sensor of the equipment mounted on the 3D imaging sonar. uuv and roll angle uuv It can correct the inclination angle of the seabed plane, thereby obtaining the seabed pitch in the geodetic coordinate system. plane '、Roll plane ':
[0021] Pitch plane '=Pitch plane +Pitch uuv ;
[0022] Roll plane =Roll plane +Roll uuv .
[0023] Furthermore, in any of the above technical solutions, the seabed tilt detection method also includes:
[0024] S0. Based on the motion information of the equipment carried by the inertial navigation system, the attitude information of the three-dimensional imaging sonar obtained by the attitude sensor of the three-dimensional imaging sonar, and the echo time of the three-dimensional imaging sonar, the position of the seabed plane is estimated and used as a reference for subsequent fitting.
[0025] In any of the above technical solutions, further, after calculating the plane equation of the three-point fitting in step S12, if there is no intersection between the plane and the seabed plane estimated in step S0 within the sonar field of view, the fitted plane is discarded, and three points are randomly selected again to fit a new fitted plane.
[0026] In any of the above technical solutions, the least squares plane fitting method in step S16 is further described as follows:
[0027] The general equation of a plane is Ax + By + Cz + D = 0, which can be transformed to obtain:
[0028]
[0029] Write the above equation in matrix form:
[0030]
[0031] For a point set, the above equation becomes:
[0032]
[0033] in and These are column vectors representing the x-axis, y-axis, and z-axis coordinates of all interior points. By solving the least-squares solution of the system of equations, the values of α, β, and γ can be obtained, thus completing the plane fitting.
[0034] The beneficial effects of this invention are:
[0035] This invention utilizes point cloud data generated by three-dimensional imaging sonar on the seabed. A single detection can calculate the tilt angle of the seabed within the field of view. It not only has small errors but also can refresh the tilt angle data in real time, resulting in high detection and calculation efficiency.
[0036] When determining the optimal fitting plane, the projection area of the 3D point cloud within a certain distance from each fitting plane on that fitting plane is calculated as a condition for determining the optimal fitting plane.
[0037] The seabed surface is estimated by using inertial navigation information, attitude information, and sonar echo time. During the fitting process, if there is no intersection between the fitted surface and the estimated seabed surface within the sonar field of view, the fitted surface is discarded, and three points are randomly selected to fit a new fitted surface. This can screen out fitted surfaces with low probability in advance, improving the speed and quality of detection. Attached Figure Description
[0038] The advantages of the above and additional aspects of the present invention will become apparent and readily understood in the description of the embodiments in conjunction with the following drawings, wherein:
[0039] Figure 1 This is a schematic flowchart of a seabed tilt detection method based on three-dimensional imaging sonar point cloud coordinates according to an embodiment of the present invention;
[0040] Figure 2 This is a scatter plot of measured point cloud data of a shallow slope based on a seabed tilt detection method based on three-dimensional imaging sonar point cloud coordinates according to an embodiment of the present invention.
[0041] Figure 3 This is a schematic diagram of the sonar coordinate system for a seabed tilt detection method based on three-dimensional imaging sonar point cloud coordinates according to an embodiment of the present invention;
[0042] Figure 4 This is a schematic diagram illustrating the tilt correction principle of a seabed tilt detection method based on three-dimensional imaging sonar point cloud coordinates according to an embodiment of the present invention.
[0043] Figure 5 This is a schematic diagram of the measurement of measured point cloud data of a shallow slope using a seabed tilt detection method based on three-dimensional imaging sonar point cloud coordinates, according to an embodiment of the present invention. Detailed Implementation
[0044] To better understand the above-mentioned objectives, features, and advantages of the present invention, the present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments. It should be noted that, unless otherwise specified, the embodiments of the present invention and the features thereof can be combined with each other.
[0045] In the following description, many specific details are set forth in order to provide a full understanding of the invention. However, the invention may also be practiced in other ways different from those described herein, and therefore the scope of protection of the invention is not limited to the specific embodiments disclosed below.
[0046] like Figure 1 As shown, this embodiment provides a method for detecting seabed tilt angle based on the coordinates of a three-dimensional imaging sonar point cloud. This method is applied to a three-dimensional imaging sonar mounted on an underwater device, and includes:
[0047] S0. The position of the seabed is estimated based on the motion information obtained by the inertial navigation system of the equipment carried by the three-dimensional imaging sonar, the attitude information obtained by the attitude sensor of the three-dimensional imaging sonar, and the echo time of the three-dimensional imaging sonar, and is used as a reference for subsequent fitting.
[0048] Specifically, the underwater equipment has an inertial navigation system, and the three-dimensional imaging sonar has an attitude sensor. The three-dimensional imaging sonar receives motion information provided by the inertial navigation system and attitude information provided by the attitude sensor. The three-dimensional imaging sonar emits sound waves and receives echoes. Based on the motion information, attitude information and echo time, it determines the depth of the seabed and uses the plane at that depth as the estimated seabed plane.
[0049] S1. Fit the optimal plane based on the three-dimensional point cloud data generated by the three-dimensional imaging sonar.
[0050] Three-dimensional point cloud data describes the position and shape of a target in space. In this invention, the target is the seabed surface. The three-dimensional point cloud data includes: the coordinates of the point, the echo intensity of the point, and the distance between the point and the sonar. Figure 2 As shown, the point cloud data of a shallow slope target can be represented by a scatter plot. The horizontal axis of the scatter plot represents the distance of each point from the sonar in the horizontal direction, and the vertical axis represents the depth of each point relative to the sonar.
[0051] Step S1 specifically includes:
[0052] S11. Establish a right-handed three-dimensional coordinate system with the three-dimensional imaging sonar as the origin. Use the orientation of the three-dimensional imaging sonar as the positive z-axis, and the vertical plane containing the z-axis as the yz plane. The positive y-axis is pointing upwards.
[0053] S12. Randomly select three points P1(x1, y1, z1), P2(x2, y2, z2) and P3(x3, y3, z3) from the three-dimensional point cloud data, and calculate the plane equation fitted by these three points: ax + by + z + c = 0, where a, b, and c are parameters.
[0054] Specifically, after calculating the plane equation fitted by three points in step S12, if there is no intersection point between this plane and the seabed plane estimated in step S0 within the sonar vision range, discard this fitted plane and randomly select three new points to fit a new plane.
[0055] S13. Set a distance threshold r, and divide all three-dimensional point clouds into inliers and outliers according to the set distance threshold r. The inliers represent the set of points located near the fitted plane: S in = {(x, y, z) ||ax + by + z + c| < r}; The outliers represent the set of points far from the fitted plane: S out = {(x, y, z) ||ax + by + z + c| > r}.
[0056] S14. Divide the fitted plane into grids of a fixed size, project all inliers onto the fitted plane, screen all grids containing the projections of inliers, and calculate the total area of the screened grids as the inlier projection area record.
[0057] S15. Repeat steps S12 to S14 for a preset number of times. After execution, select the fitted plane with the largest recorded inlier projection area.
[0058] S16. Perform least squares fitting on the inliers of the fitted plane selected in step S15, and use the refined fitted plane as the output plane.
[0059] Among them, the method of least squares plane fitting is as follows:
[0060] The general equation of the plane is Ax + By + Cz + D = 0, and it is transformed as follows:
[0061]
[0062] Write the above formula in matrix form:
[0063]
[0064] For the point set, the above formula becomes:
[0065]
[0066] Among them and These are column vectors representing the x-axis, y-axis, and z-axis coordinates of all interior points. By solving the least-squares solution of the system of equations, the values of α, β, and γ can be obtained, thus completing the plane fitting.
[0067] S2. Calculate the tilt angle of the output plane relative to the sonar coordinate system, and then perform angle correction based on the tilt angle of the sonar in the vertical plane to solve the tilt angle of the output plane relative to the geodetic coordinate system.
[0068] In the sonar coordinate system, the tilt angle θ of the output plane is the normal vector of that plane. unit vector with the y-axis The included angle,
[0069] In the sonar coordinate system, the pitch angle is obtained by projecting the normal of the output plane onto the YZ plane. plane Project the plane normal onto the XY plane to obtain the roll angle. plane .
[0070] The calculated tilt angle is corrected for geodetic coordinates and combined with the sonar pitch angle output from the attitude sensor of the equipment mounted on the 3D imaging sonar. uuv and roll angle uuv It can correct the inclination angle of the seabed plane, thereby obtaining the seabed pitch in the geodetic coordinate system. plane '、Roll plane '.
[0071] Seafloor Pitch in Geodetic Coordinate System plane '、Roll plane 'These are the sum of the seabed tilt angle in the sonar coordinate system and the sonar tilt angle output by the attitude sensor:'
[0072] Pitch plane '=Pitch plane +Pitch uuv ;
[0073] Roll plane =Roll plane +Roll uuv ;
[0074] The principle is as follows Figure 4 As shown, in the YZ plane of the sonar coordinate system, the sonar pitch angle output by the attitude sensor is... uuv Let be the angle between the z-axis of the sonar coordinate system and the horizontal plane. According to geometry, this angle is equal to the angle between the y-axis of the sonar coordinate system and the normal to the horizontal plane (the y-axis of the geodetic coordinate system). Therefore, we can directly add it to the tilt angle of the sonar coordinate system to correct it to the tilt angle of the geodetic coordinate system. The same applies to the XY plane of the sonar coordinate system.
[0075] like Figure 5 As shown, the detection method provided by this invention is applied to... Figure 2 A test was conducted on a shallow slope. Figure 5 The light and medium-colored point cloud part corresponds to Figure 2 The shallow slope plane in the image is represented by the dark plane, which is the plane fitted according to the algorithm in this paper, i.e., the fitted seabed plane. The subsequent calculation of the seabed dip angle is to calculate the dip angle of this plane.
[0076] In summary, this invention provides a method for detecting seabed tilt angle based on three-dimensional imaging sonar point cloud coordinates. This method is applied to three-dimensional imaging sonar mounted on underwater equipment and includes:
[0077] S0. The position of the seabed is estimated based on the motion information obtained by the inertial navigation system of the equipment carried by the three-dimensional imaging sonar, the attitude information obtained by the attitude sensor of the three-dimensional imaging sonar, and the echo time of the three-dimensional imaging sonar, and is used as a reference for subsequent fitting.
[0078] S1. Fit multiple planes based on the 3D point cloud data generated by the 3D imaging sonar, and select the best plane as the output plane based on the distribution of the 3D point cloud.
[0079] S2. Calculate the tilt angle of the output plane relative to the sonar coordinate system, and then perform angle correction based on the tilt angle of the sonar in the vertical plane to solve the tilt angle of the output plane relative to the geodetic coordinate system.
[0080] In step S1, after fitting each plane, the shortest distance between all 3D point clouds and the fitted plane is calculated. All 3D point clouds with a distance less than the distance threshold r are set as points in the fitted plane.
[0081] The fitting plane is divided into a grid of fixed size. All interior points are projected onto the fitting plane. All grids containing the interior point projections are filtered out. The total area of the filtered grids is calculated and recorded as the interior point projection area. The fitting plane with the largest interior point projection area is selected.
[0082] The steps in this invention can be adjusted, combined, or deleted according to actual needs.
[0083] The units in the device of the present invention can be merged, divided, or reduced according to actual needs.
[0084] Although the invention has been disclosed in detail with reference to the accompanying drawings, it should be understood that these descriptions are merely exemplary and not intended to limit the application of the invention. The scope of protection of the invention is defined by the appended claims and may include various modifications, alterations, and equivalents made to the invention without departing from the scope and spirit of the invention.
Claims
1. A method for detecting seabed tilt angle based on three-dimensional imaging sonar point cloud coordinates, the method being applied to a three-dimensional imaging sonar mounted on an underwater device, characterized in that... The method includes: S1. Fit multiple planes based on the three-dimensional point cloud data generated by the three-dimensional imaging sonar, and select the best plane as the output plane based on the distribution of the three-dimensional point cloud. S2. Calculate the tilt angle of the output plane relative to the sonar coordinate system, and then perform angle correction based on the tilt angle of the sonar in the vertical plane to solve the tilt angle of the output plane relative to the geodetic coordinate system. In step S1, after fitting each plane, the shortest distance between all three-dimensional point clouds and the fitted plane is calculated. All three-dimensional point clouds with a distance less than the distance threshold r are set as points in the fitted plane. The fitted plane is divided into a grid of fixed size. All interior points are projected onto the fitted plane. All grids containing the interior point projections are filtered out. The total area of the filtered grids is calculated and recorded as the interior point projection area. The fitted plane with the largest interior point projection area is selected as the best plane.
2. The seabed tilt detection method based on three-dimensional imaging sonar point cloud coordinates as described in claim 1, characterized in that, Step S1 includes: S11. Establish a right-handed three-dimensional coordinate system with the three-dimensional imaging sonar as the origin, and use the orientation of the three-dimensional imaging sonar as the positive direction of the z-axis. The vertical plane containing the z-axis is used as the yz plane, and the positive direction of the y-axis is upward. S12. Randomly select three points P1(x1,y1,z1), P2(x2,y2,z2) and P3(x3,y3,z3) in the three-dimensional point cloud data, and calculate the plane equation fitted by the three points: ax + by + z + c = 0, where a, b and c are parameters; S13. Set a distance threshold r, and divide all three-dimensional point clouds into inliers and outliers according to the set distance threshold r, where the inliers represent the set of points located near the fitted plane: S in = {(x, y, z) ||ax + by + z + c| < r}; the outliers represent the set of points far from the fitted plane: S out = {(x, y, z) ||ax + by + z + c| > r}; S14. Divide the fitting plane into a grid of fixed size, project all interior points onto the fitting plane, filter all grids that contain the projections of interior points, and calculate the total area of the filtered grids as the projection area of interior points. S15. Repeat steps S12 to S14 a preset number of times. After the execution is complete, select the fitting plane with the largest recorded interior point projection area. S16. Perform least-squares fitting on the interior points of the fitting plane selected in step S15, and use the refined fitting plane as the output plane.
3. The seabed tilt detection method based on three-dimensional imaging sonar point cloud coordinates as described in claim 1, characterized in that, Step S2 specifically includes: In the sonar coordinate system, the pitch angle can be obtained by projecting the normal of the output plane onto the YZ plane. plane Similarly, by projecting the plane normal onto the XY plane, the yaw angle Roll can be obtained. plane ; The calculated tilt angle is corrected for geodetic coordinates and combined with the sonar pitch angle output from the attitude sensor of the equipment mounted on the 3D imaging sonar. uuv and roll angle uuv It can correct the inclination angle of the seabed plane, thereby obtaining the seabed pitch in the geodetic coordinate system. plane '、Roll plane ': Pitch plane ’=Pitch plane +Pitch uuv ; Roll plane ’=Roll plane +Roll uuv 。 4. The seabed tilt detection method based on three-dimensional imaging sonar point cloud coordinates as described in claim 2, characterized in that, The seabed tilt angle detection method also includes: S0. Based on the motion information of the equipment carried by the inertial navigation system, the attitude information of the three-dimensional imaging sonar obtained by the attitude sensor of the three-dimensional imaging sonar, and the echo time of the three-dimensional imaging sonar, the position of the seabed plane is estimated and used as a reference for subsequent fitting.
5. The seabed tilt detection method based on three-dimensional imaging sonar point cloud coordinates as described in claim 4, characterized in that, After calculating the plane equation of the three-point fitting in step S12, if there is no intersection between the plane and the seabed plane estimated in step S0 within the sonar field of view, the fitted plane is discarded, and three points are randomly selected again to fit a new fitted plane.
6. The seabed tilt detection method based on three-dimensional imaging sonar point cloud coordinates as described in claim 2, characterized in that, The least squares plane fitting method in step S16 is as follows: The general equation of a plane is Ax + By + Cz + D = 0, which can be transformed to obtain: Write the above equation in matrix form: For a point set, the above equation becomes: in and These are column vectors representing the x-axis, y-axis, and z-axis coordinates of all interior points. By solving the least-squares solution of the system of equations, the values of α, β, and γ can be obtained, thus completing the plane fitting.