Unmanned surface vehicle obstacle detection method based on laser radar

Abstract

The invention relates to an unmanned surface vehicle obstacle detection method based on a laser radar. The method comprises the following steps: establishing a grid map; laser radar point cloud data are processed on the grid map, three-dimensional point cloud data are converted into two-dimensional data, and rapid processing of the laser radar point cloud data is carried out; performing multi-frame point cloud data fusion on the grid map by using the processed data; and an improved DBSCAN clustering algorithm is used to carry out filtering processing and clustering on the point cloud data after fusion of the multiple frames of point cloud data, and target detection of the laser radar point cloud data is realized. According to the invention, in a complex sea surface environment, the laser radar can be used to carry out efficient and real-time detection on water surface obstacles, and environment sensing algorithm support is provided for autonomous navigation of a water surface unmanned ship path.

Description

Technical Field

[0001] The present invention belongs to the technical field of unmanned ship control, and in particular to a method for detecting obstacles of an unmanned surface boat based on laser radar. Background Art

[0002] Unmanned surface vehicles are a kind of general-purpose intelligent surface mission platform that is increasingly widely used. They have the characteristics of small size, low cost, long endurance, flexibility, high speed, high energy efficiency, and adaptability to harsh environments. Unmanned surface vehicles play an increasingly important role in the fields of marine environment measurement, seabed topography measurement, marine resource development, etc., and have broad application prospects.

[0003] The primary task of autonomous local path planning (collision avoidance planning) for surface unmanned boats is to detect obstacles in the sea. Ordinary ships are usually equipped with optoelectronic equipment (visible light, infrared) and ocean radar sensors. Optoelectronic equipment outputs time series image information with high refresh rate and high target resolution; ocean radar outputs scanning images with long detection distance and can provide navigation all day long. However, both sensors have their own limitations and it is difficult to fully meet the environmental sensing needs of surface unmanned boats. Especially in complex waters, close target distance and fast speed, the navigation radar information update frequency is slow, the resolution is low, and the target features are few. In near-shore scenes such as docks, there are many sea clutters, and it is easy to miss and lose targets. Optoelectronic equipment is a passive detection device, which is very sensitive to weather and light changes. The essence of optical imaging is the two-dimensional image plane projection of the three-dimensional world. It lacks target distance information and real size, and it is difficult to meet the needs of obstacle detection and positioning. Therefore, it is difficult for optoelectronic equipment (visible light, infrared) and ocean radar sensors to provide reliable obstacle detection and identification support for surface unmanned boats.

[0004] LiDAR has the advantages of both ocean radar and optical imaging. It can not only obtain the distance and azimuth information of the target for target positioning and navigation obstacle avoidance, but also obtain the three-dimensional contour information of the target for obstacle segmentation and identification. At the same time, LiDAR uses active laser ranging, is not easily affected by light and weather, has a fast scanning frequency, and is very suitable for the detection and identification of medium and short-range targets on unmanned platforms.

[0005] LiDAR is an active sensor that is rich in information and is not affected by weather or lighting. It can obtain three-dimensional information of the scene and is a necessary means for unmanned boats to detect targets and measure their size, distance, and speed. It is of great value for unmanned boat navigation, obstacle avoidance, target positioning, and tracking. However, the point cloud of LiDAR sensors is sparse, making it difficult to detect small targets and susceptible to interference from sea clutter. The working environment of surface unmanned boats is mostly offshore, on mudflats, and even inland rivers, with complex and changeable backgrounds and strong target interference. Therefore, how to perform effective obstacle detection based on LiDAR sensors under complex sea conditions and strong interference conditions is the key to the perception of the surface environment of unmanned boats. Summary of the invention

[0006] The purpose of the present invention is to overcome the shortcomings of the prior art and propose a surface unmanned boat obstacle detection method based on laser radar, which can use laser radar to perform efficient and real-time detection of surface obstacles in complex sea environments, and provide environmental perception algorithm support for the autonomous navigation of surface unmanned boat paths.

[0007] The present invention solves the technical problem by adopting the following technical solutions:

[0008] A method for detecting obstacles on a surface unmanned boat based on laser radar comprises the following steps:

[0009] Step 1: Create a grid map;

[0010] Step 2: Process the LiDAR point cloud data on the grid map, convert the three-dimensional point cloud data into two-dimensional data, and perform rapid processing of the LiDAR point cloud data;

[0011] Step 3: Use the processed data to perform multi-frame point cloud data fusion on the grid map;

[0012] Step 4: Use the improved DBSCAN clustering algorithm to filter the point cloud data after fusion of multi-frame point cloud data to complete the detection of various obstacles.

[0013] Moreover, the specific implementation method of step 1 is: take the laser radar as the coordinate origin, the carrier forward direction is the positive direction of the X axis, the right hand side perpendicular to the forward direction is the positive direction of the Y axis, and the X-axis and Y-axis cutoff distances are set to be X and Y, respectively, according to the laser radar range. turn With Y turn , the grid size is g cell , input point cloud P = {P i |P i =(x i ,y i ,z i ), i=1,2,...,N} for XY plane projection and rasterization, where any point p in the point cloudi Coordinate calculation method on raster map:

[0014] g x =[(x+X turn ) / g cell ]

[0015] g y =[(y+Y turn ) / g cell ]

[0016] Among them, g x is the X-axis coordinate value of the grid map, g y is the Y-axis coordinate value of the raster map, x is the X-axis coordinate value of the lidar coordinate system, and y is the Y-axis coordinate value of the lidar coordinate system.

[0017] The grid map is represented as a set G = {G x,y =(g x ,g y )|g x =0,1,...,H,g y =0,1,...,W}, and its row resolution is H = [2*X turn / g cell ], the column resolution is W = [2*Y turn / g cell ];

[0018] Moreover, the specific implementation method of step 3 is: using 3 frames of point cloud data for fusion, wherein the 3 frames of point cloud data processing process is to fuse 1 frame of point cloud data again based on the fusion processing of 2 frames of point cloud data.

[0019] Moreover, the specific implementation method of the two-frame point cloud data fusion is as follows: X0, Y0, Z0 represent the three-dimensional coordinates of the current point cloud, X V , Y V , Z V Represents the three-dimensional coordinates of the previous frame of point cloud; Δφ, Δθ, Δψ represent the yaw angle, pitch angle and roll angle of the two frames of point cloud. When converted to the lidar coordinate system, the yaw, pitch and roll angles of the carrier relative to the global coordinate system are combined with the translation amounts ΔN, ΔE, ΔZ in the east, north and sky directions to solve the translation amounts Δx, Δy, Δz in the lidar coordinate system:

[0020]

[0021] Among them, R is the coordinate rotation matrix, c ψ =cos(Δψ) is the cosine value of the yaw angle between the two frames of point cloud, c θ =cos(Δθ) is the cosine value of the pitch angle between the two frames of point cloud, cφ =cos(Δφ) is the cosine value of the rolling angle between the two frames of point cloud, s ψ =sin(Δψ) is the sine value of the yaw angle between the two frames of point cloud, s θ =sin(Δθ) is the sine value of the pitch angle between the two frames of point cloud, s φ =sin(Δφ) is the sine value of the rolling angle of the two frame point clouds. Δx, Δy, Δz in the above formula represent the offset of the laser radar in the three directions of xyz in the radar coordinate system; B(x, y, z) represents the point cloud coordinates before rotation and translation transformation, and B'(x', y', z') represents the point cloud coordinates after transformation.

[0022] Moreover, the specific implementation method of step 4 is: according to the laser radar scanning line distribution, the distance between two adjacent scanning lines is calculated to establish ε * The corresponding list value is then found in the list according to the coordinate value of each scanning point. Finally, the corresponding neighborhood value is determined by linear interpolation, and the KD-tree neighborhood search method is used to search and complete the detection of each obstacle.

[0023] The advantages and positive effects of the present invention are:

[0024] The present invention establishes a grid map; processes the laser radar point cloud data on the grid map, converts the three-dimensional point cloud data into two-dimensional data, and performs rapid processing of the laser radar point cloud data; uses the processed data to fuse multi-frame point cloud data on the grid map; uses the improved DBSCAN clustering algorithm to filter the point cloud data after the fusion of multi-frame point cloud data to complete the detection of various obstacles. The present invention can use laser radar to perform efficient and real-time detection of surface obstacles in complex sea environments, and provide environmental perception algorithm support for autonomous navigation of surface unmanned boat paths. BRIEF DESCRIPTION OF THE DRAWINGS

[0025] Figure 1 A diagram of a laser radar-based navigation obstacle detection system for an unmanned surface boat of the present invention;

[0026] Figure 2 Schematic diagram of rotation fusion (left) and translation fusion (right);

[0027] Figure 3 A schematic diagram of the search process of the DBSCAN algorithm of the present invention;

[0028] Figure 4 Schematic diagram of laser radar scanning line

[0029] Figure 5 This is a flow chart of the surface unmanned boat navigation obstacle target detection algorithm of the present invention;

[0030] Figure 6 It is a flow chart of the KD-tree neighborhood search process of the present invention;

[0031] Figure 7 This is a rendering of an unmanned surface boat equipped with laser radar;

[0032] Figure 8 This is a diagram showing the effect of the laser radar of the present invention detecting ships on the sea surface. DETAILED DESCRIPTION

[0033] The present invention is further described in detail below with reference to the accompanying drawings.

[0034] A surface unmanned boat obstacle detection method based on laser radar, such as Figure 5 As shown, the following steps are included:

[0035] Step 1: Create a grid map.

[0036] With the laser radar as the coordinate origin, the carrier's forward direction is the positive direction of the X axis, and the right hand side perpendicular to the forward direction is the positive direction of the Y axis. The X-axis and Y-axis cutoff distances are set to X and Y respectively according to the laser radar range. turn With Y turn , the grid size is g cell , input point cloud P = {P i |P i =(x i ,y i ,z i ), i=1,2,...,N} for XY plane projection and rasterization, where any point p in the point cloud i Coordinate calculation method on raster map:

[0037] g x =[(x+X turn ) / g cell ]

[0038] g y =[(y+Y turn ) / g cell ]

[0039] Among them, g x is the X-axis coordinate value of the grid map, g y is the Y-axis coordinate value of the raster map, x is the X-axis coordinate value of the lidar coordinate system, and y is the Y-axis coordinate value of the lidar coordinate system.

[0040] The grid map is represented as a set G = {G x,y =(g x ,g y )|g x =0,1,...,H,g y=0,1,...,W}, and its row resolution is H = [2*X turn / g cell ], the column resolution is W = [2*Y turn / g cell ];

[0041] Step 2: Process the LiDAR point cloud data on the grid map, convert the three-dimensional point cloud data into two-dimensional data, and quickly process the LiDAR point cloud data.

[0042] Step 3: Use the processed data to perform multi-frame point cloud data fusion on the grid map.

[0043] The laser radar preprocessing algorithm of multi-frame point cloud data fusion is used to improve the real-time monitoring accuracy of dynamic obstacles at sea. Considering the speed of the surface unmanned boat and the speed of dynamic obstacles, the autonomous navigation safety of the surface unmanned boat is ensured, and the surface unmanned boat is required to have a high real-time performance of marine obstacle detection. The fusion of 3-frame point cloud data ensures both the obstacle detection accuracy and good real-time performance. The 3-frame point cloud data processing process is to fuse 1 frame of point cloud data again on the basis of the fusion processing of 2 frames of point cloud data, and the effect is better than that of 2-frame point cloud data fusion.

[0044] Three frames of point cloud data are used for fusion, wherein the processing process of the three frames of point cloud data is to fuse one frame of point cloud data again based on the fusion processing of the two frames of point cloud data.

[0045] like Figure 2 As shown in the figure, the specific implementation method of fusion of two frames of point cloud data is: X0, Y0, Z0 represent the three-dimensional coordinates of the current point cloud, X V , Y V , Z V Represents the three-dimensional coordinates of the previous frame of point cloud; Δφ, Δθ, Δψ represent the yaw angle, pitch angle and roll angle of the two frames of point cloud. When converted to the lidar coordinate system, the yaw, pitch and roll angles of the carrier relative to the global coordinate system are combined with the translation amounts ΔN, ΔE, ΔZ in the east, north and sky directions to solve the translation amounts Δx, Δy, Δz in the lidar coordinate system:

[0046]

[0047] Among them, R is the coordinate rotation matrix, c ψ =cos(Δψ) is the cosine value of the yaw angle between the two frames of point cloud, c θ =cos(Δθ) is the cosine value of the pitch angle between the two frames of point cloud, c φ =cos(Δφ) is the cosine value of the rolling angle between the two frames of point cloud, s ψ =sin(Δψ) is the sine value of the yaw angle between the two frames of point cloud, sθ =sin(Δθ) is the sine value of the pitch angle between the two frames of point cloud, s φ =sin(Δφ) is the sine value of the rolling angle of the two frame point clouds. Δx, Δy, Δz in the above formula represent the offset of the laser radar in the three directions of xyz in the radar coordinate system; B(x, y, z) represents the point cloud coordinates before rotation and translation transformation, and B'(x', y', z') represents the point cloud coordinates after transformation.

[0048] Step 4: Use the improved DBSCAN clustering algorithm to filter the point cloud data after fusion of multi-frame point cloud data to complete the detection of various obstacles.

[0049] like Figure 3 As shown, the DBSCAN algorithm has two important parameters:

[0050] Eps: Eps is the neighborhood search radius for determining whether a point is a core point.

[0051] Minpts: Minpts refers to the minimum number of points other than the core point contained in the Eps neighborhood radius when a point is a core point.

[0052] According to the laser radar scanning line distribution, the distance between two adjacent scanning lines is calculated to establish Eps * The corresponding list value is then found in the list according to the coordinate value of each scanning point. Finally, the corresponding neighborhood value is determined by linear interpolation, and the KD-tree neighborhood search method is used to search and complete the detection of each obstacle.

[0053] Eps * The method of establishing the list is as follows: the laser radar has a total of n scanning lines, and the height of the laser radar origin installed on the unmanned boat from the horizontal plane is h, such as Figure 4 As shown, the pitch angle of the scan line constitutes a set α = {α i}(i=1,2,3,...,n), the radial distance from the laser radar scanning line projection point to point O constitutes a set r={r i}(i=1,2,3,...,n), the distances between the projection points of two adjacent laser scanning lines constitute a set Eps={Eps i}(i=1,2,3,...,n-1)r, the corresponding element of Eps satisfies r i =h / tan(α i )(i=1,2,3,...,n),Eps i =r i+1 -r i (i=1,2,3,...,n).

[0054] The installation height of the laser radar on the mast of the surface unmanned boat is h = 2.5 from the water surface, so the set r is modified to r i =2.5 / tan(α i )(i=1,2,3,...,n), denoted as r * ; The corresponding set Eps is modified to Record as list Eps * .

[0055] Eps * How to use the list: Assume there is a three-dimensional space point P (x, y, z), the distance to the z axis Corresponding In r * Search in list Make In Eps * The corresponding According to the first-order linear interpolation method, the Eps value at any point P(x,y,z) is:

[0056]

[0057] Among them, κ is the correction coefficient, which is generally 1.05. Eps0 is the lower limit of the neighboring threshold, which is generally Eps0=0.37.

[0058] The number of points contained in the area of ​​each point Eps in the statistical data set is calculated and used as the density threshold Minpts.

[0059]

[0060] like Figure 6 As shown in Figure 1, the KD-tree neighborhood search process is as follows: randomly select a point cloud from the point cloud data as the search starting point. First, start the search from vertex T6, calculate the distance between the starting point O and T6, and use it as the minimum distance L min Since the x value of the starting point is less than point T6, the next search starts from the left subtree, that is, the distance L1 from T2 to the starting point O is calculated and compared with the previous minimum distance L min For comparison, since L1 is smaller than L min , so the minimum distance is updated. Because the y value of the starting point O is greater than point T2, the next step of the search is to enter the right subtree, that is, to calculate the distance L2 from the starting point O to T4 and compare it with the previous minimum distance L min For comparison, since L2 is greater than L min , so the minimum distance is not updated. From now on, the "backtracking" search operation is started, returning to T4's parent node T2, and continuing the search in the opposite direction of the previous one. The distance L3 between the starting point O and T1 is calculated and compared with the previous minimum distance L minFor comparison, since L3 is greater than L min , so the updated minimum distance is L min . Continue to "backtrack" to node T6 and find that the circle L with the starting point O as the center min The circle with a radius of has no intersection with the hyperplane passing through point T6, that is, point T1 is the point closest to the red point in the data set, and the search ends. This search method avoids searching each point one by one, greatly speeding up the search efficiency.

[0061] According to the above-mentioned laser radar-based obstacle detection method for unmanned surface boats, a Figure 1 The laser radar-based surface unmanned boat obstacle detection system shown includes a laser radar, an integrated navigation, and a DSP reinforced computer board, wherein the laser radar and the integrated navigation are respectively connected to the DSP reinforced computer board.

[0062] Among them, the laser radar uses the domestic Pandar40 laser radar sensor. The perception unit of the Pandar40 laser radar contains a total of 40 lasers, and has a 360-degree horizontal scanning field of view and a 24-degree vertical scanning field of view, which can provide nearly one million measurement data points per second. The combined navigation is GPS and inertial navigation, which is composed of fiber-optic gyroscopes, accelerometers and GNSS receiving boards. Through multi-sensor fusion and navigation analysis algorithms, it can achieve accurate measurement of attitude, heading and position information in the application field of high-precision mobile measurement systems. The DSP reinforced computer board, which serves as the operating platform for the laser radar detection algorithm, is designed with a computer with 32GB memory, i7 processor + dsp6678, and a main frequency of 2.6GHz, and is designed in the form of a 6U CPCI reinforced computer board.

[0063] like Figure 7 The following is a rendering of an unmanned surface boat equipped with a laser radar. A small unmanned surface boat is used as a platform for laser radar test. The platform uses a single main engine and a single jet pump power mode, and has good controllability. Figure 8 The figure shows that the surface unmanned boat is equipped with a laser radar for sea trials and verification. The distance between multiple surface ships and the unmanned boat is 127-170 meters. It can be seen from Figure 8 that the laser radar can detect these long-distance sea target ships, which illustrates the actual engineering effect of this algorithm and solves the problem of sea obstacle detection for surface unmanned boats.

[0064] It should be emphasized that the embodiments described in the present invention are illustrative rather than restrictive. Therefore, the present invention includes but is not limited to the embodiments described in the specific implementation manner. Any other implementation manners derived by those skilled in the art based on the technical solution of the present invention also fall within the scope of protection of the present invention.

Claims

1. A method for detecting obstacles on a surface unmanned boat based on laser radar, characterized in that: The following steps are involved: Step 1: Create a grid map; Step 2: Process the LiDAR point cloud data on the grid map, convert the three-dimensional point cloud data into two-dimensional data, and perform rapid processing of the LiDAR point cloud data; Step 3: Use the processed data to perform multi-frame point cloud data fusion on the grid map; Step 4: Use the improved DBSCAN clustering algorithm to filter the point cloud data after fusion of multi-frame point cloud data to complete the detection of various obstacles.

2. The method for detecting obstacles on a surface unmanned boat based on laser radar according to claim 1, characterized in that: The specific implementation method of step 1 is: take the laser radar as the coordinate origin, the carrier's forward direction is the positive direction of the X axis, the right hand side perpendicular to the forward direction is the positive direction of the Y axis, and the X-axis and Y-axis cutoff distances are set to X and Y respectively according to the laser radar range. turn With Y turn , the grid size is g cell , input point cloud P = {P i |P i =(x i ,y i ,z i ), i=1,2,...,N} for XY plane projection and rasterization, where any point p in the point cloud i Coordinate calculation method on raster map: g x =[(x+X turn ) / g cell ] g y =[(y+Y turn ) / g cell ] Among them, g x is the X-axis coordinate value of the grid map, g y is the Y-axis coordinate value of the grid map, x is the translation of the X-axis coordinate axis of the laser radar coordinate system, and y is the translation of the Y-axis coordinate axis of the laser radar coordinate system; The grid map is represented as a set G = {G x,y =(g x ,g y )|g x =0,1,...,H,g y =0,1,...,W}, and its row resolution is H = [2*X turn / g cell ], the column resolution is W = [2*Y turn / g cell ].

3. The method for detecting obstacles on a surface unmanned boat based on laser radar according to claim 1, characterized in that: The specific implementation method of step 3 is: using 3 frames of point cloud data for fusion, wherein the 3 frames of point cloud data processing process is to fuse 1 frame of point cloud data again based on the fusion processing of 2 frames of point cloud data.

4. The method for detecting obstacles on a surface unmanned boat based on laser radar according to claim 3, characterized in that: The specific implementation method of the fusion of the two-frame point cloud data is as follows: X0, Y0, and Z0 represent the three-dimensional coordinates of the current point cloud. V , Y V , Z V Represents the three-dimensional coordinates of the previous frame of point cloud; Δφ, Δθ, Δψ represent the yaw angle, pitch angle and roll angle of the two frames of point cloud. When converted to the lidar coordinate system, the yaw, pitch and roll angles of the carrier relative to the global coordinate system are combined with the translation amounts ΔN, ΔE, ΔZ in the east, north and sky directions to solve the translation amounts Δx, Δy, Δz in the lidar coordinate system: Among them, R is the coordinate rotation matrix, c ψ =cos(Δψ) is the cosine value of the yaw angle between the two frames of point cloud, c θ =cos(Δθ) is the cosine value of the pitch angle between the two frames of point cloud, c φ =cos(Δφ) is the cosine value of the rolling angle between the two frames of point cloud, s ψ =sin(Δψ) is the sine value of the yaw angle between the two frames of point cloud, s θ =sin(Δθ) is the sine value of the pitch angle between the two frames of point cloud, s φ =sin(Δφ) is the sine value of the rolling angle of the two frame point clouds. Δx, Δy, Δz in the above formula represent the offset of the laser radar in the three directions of xyz in the radar coordinate system; B(x, y, z) represents the point cloud coordinates before rotation and translation transformation, and B'(x', y', z') represents the point cloud coordinates after transformation.

5. The method for detecting obstacles on a surface unmanned boat based on laser radar according to claim 1, characterized in that: The specific implementation method of step 4 is: according to the laser radar scanning line distribution, the distance between two adjacent scanning lines is calculated to establish Eps * The corresponding list value is then found in the list according to the coordinate value of each scanning point. Finally, the corresponding neighborhood value is determined by linear interpolation, and the KD-tree neighborhood search method is used to search and complete the detection of each obstacle.

Images