Passable area evaluation method based on three-dimensional point cloud B-spline surface

Abstract

The application relates to a passable area evaluation method based on three-dimensional point cloud B-spline surface processing, B-spline surface fitting is performed on three-dimensional point cloud data, the surrounding environment of a mobile robot is expressed in a parameterized form, the environment is reconstructed in a surface smoothing mode, the gradient of any point on a map is calculated through the parameterized surface after fitting, and the passability is evaluated based on the information and the passing ability of different robots; the parameterized surface form is adopted to continuously represent the passing difficulty, and finally, a point cloud map with a passable cost label is obtained. The B-spline surface fitting is used to solve the passable area evaluation problem of the mobile robot in the outdoor natural environment, the parameterized surface environment expression can utilize the scene continuity feature and save certain storage space, in addition, the reasonable simplification of the area passing cost standard under the condition assumption of the natural environment guarantees the calculation efficiency in the passable cost evaluation stage.

Description

Technical Field

[0001] This invention relates to the field of environmental perception technology, and in particular to a method for assessing passable areas based on B-spline surface modeling of three-dimensional point clouds. Background Technology

[0002] With the continuous development of mobile robot technologies, the operational scenarios of mobile robots are gradually expanding from indoor structured environments to large-scale unstructured outdoor environments. However, ensuring the safety of mobile robot trajectories in natural terrain environments is a key concern. Mobile robots can only generate safe trajectories by first assessing traversable areas in the environment; therefore, the identification and delineation of traversable areas in natural scenes are crucial for the safety of mobile robot movement.

[0003] In engineering, typical assessments of navigable areas for mobile robots based on laser point clouds often employ either the original single-frame laser point cloud data or the local point cloud map composed of multiple frames.

[0004] The evaluation method for raw single-frame laser point cloud data generally involves detecting abrupt changes in the point cloud along the same straight line to determine the road surface smoothness and obstacle distribution. This method is fast and efficient, suitable for operation scenarios where mobile robots operate at high speeds. However, this approach is limited by the number of laser lines; when the number of laser lines is low, the coverage area is limited, and it cannot adequately meet the requirements for trafficability assessment. Furthermore, this method is only suitable for structured road surfaces with obvious abrupt changes and is not universally applicable in environments with strong continuity and predominantly natural terrain.

[0005] The accessibility assessment method based on local point cloud maps first requires simultaneous robot localization and mapping to obtain a denser local point cloud map compared to a single-frame point cloud. Then, geometric features of the environment are extracted from the map to assess the accessibility of the mobile robot. Compared to single-frame laser point cloud data, dense point cloud maps provide higher resolution assessment results. However, accessibility assessment in large outdoor scenes often results in long computation times, posing a challenge to the real-time performance of the algorithm. In practice, increasing the grid size is often used to reduce computation, but increasing the grid size reduces the resolution of accessible areas in the map, affecting the accuracy of the assessment. Summary of the Invention

[0006] The technical problem to be solved by the present invention is to overcome the shortcomings of the prior art and provide a method for evaluating passable areas based on the B-spline surfaceization of three-dimensional point clouds.

[0007] This invention is achieved through the following technical solution: A method for evaluating passable regions based on 3D point cloud B-spline surface modeling, characterized by the following steps: S1. Construct a point cloud map during the robot's movement to obtain the original 3D point cloud map; S2. Perform data preprocessing on the original 3D point cloud map, the data preprocessing including outlier removal and point cloud downsampling; S3. Perform B-spline surface fitting on the preprocessed 3D point cloud map to obtain the control point array. ; S4. Calculate the gradient at any point on the 3D point cloud map using the fitted B-spline surface; S5. Calculate the area's traversability cost by combining the mobile robot platform's environmental mobility capabilities.

[0008] According to the above technical solution, preferably, the "removal of outliers" in step S2 includes: S211. For each point in the original three-dimensional point cloud map, search for N nearest points centered at that point to obtain a set of nearest points; S212. Calculate the average distance and standard deviation from the point to all nearby points in the set of nearby points; S213. The distance from each neighboring point to the center point is compared with the standard deviation in turn, and the outliers are deleted from the original three-dimensional point cloud map.

[0009] According to the above technical solution, preferably, the "point cloud downsampling" in step S2 includes: S221. The original three-dimensional point cloud map is segmented, and each segmented point cloud is contained in a map with length, width and height of l. l l w l h In the rectangular voxel; S222. The point cloud is divided into L*W*H voxel grids, where: x max y max z max It is the maximum position coordinate of a point in a point cloud map in three directions, x min y min z min It represents the minimum position coordinates of a point in a point cloud map in three directions; S223. Calculate the average coordinates of all points within each voxel grid, and replace all points in the voxel grid with the average coordinates as the new points.

[0010] According to the above technical solution, preferably, step S4 includes: S41. Based on the control point array The 3D point cloud map is modeled using B-spline surface modeling to obtain a series of B-spline surface patches. ; S42. At any position on each surface patch Calculate the gradient of the surface at that location.

[0011] According to the above technical solution, preferably, in step S1, the laser SLAM algorithm LIO-mapping is used to construct a point cloud map during the robot's movement.

[0012] According to the above technical solution, preferably, it also includes step S6: mapping the traversability cost of the area to point cloud intensity values ​​to obtain a three-dimensional point cloud map with traversability cost label information.

[0013] The beneficial effects of this invention are: This invention proposes a method for assessing the accessibility of natural scenes based on B-spline surface modeling of 3D point clouds. By using B-spline surface fitting, it solves the problem of assessing the accessibility of mobile robots in outdoor natural environments. Based on the parametric surface environment representation, it can utilize the continuity features of the scene and save certain storage space. In addition, the assumptions about the natural environment conditions reasonably simplify the regional access cost standard, ensuring computational efficiency in the accessibility cost assessment stage. Attached Figure Description

[0014] Figure 1 This is a flowchart of the method of the present invention.

[0015] Figure 2 This is a preprocessed 3D point cloud map.

[0016] Figure 3 A 3D point cloud map (top view) with access cost information.

[0017] Figure 4 A 3D point cloud map (main view) with passage cost information. Detailed Implementation

[0018] To enable those skilled in the art to better understand the technical solutions of the present invention, the present invention will be further described in detail below with reference to the accompanying drawings and preferred embodiments. All other embodiments obtained by those skilled in the art based on the embodiments of the invention without inventive effort are within the scope of protection of the invention.

[0019] In the specific implementation of this invention, a 32-line lidar sensor is used, which generates a three-dimensional point cloud with a frequency of 10Hz and a detection range of up to 100 meters. During data acquisition using a mobile robot platform, the travel speed is approximately 1.2 meters per second.

[0020] like Figure 1 As shown, the present invention includes the following steps: S1. Using the laser SLAM algorithm LIO-mapping, a point cloud map is constructed during the robot's movement to obtain the original 3D point cloud map. LIO-mapping is a method that tightly couples laser and IMU, minimizing the measurement errors of the laser radar and IMU to obtain a globally consistent pose estimate for the robot, while simultaneously mapping the surrounding environment.

[0021] S2. Perform data preprocessing on the original 3D point cloud map, including removing outliers and downsampling the point cloud.

[0022] (1) Remove outliers: Errors in lidar measurements can generate outliers, which hinder subsequent point cloud processing. Therefore, these outliers are first removed. For each point in a given 3D point cloud map, the search distance is centered on that point. Find the nearest points and obtain the set of nearest points. Then calculate the average distance and standard deviation from that point to all its nearest neighbors:

[0023]

[0024] in Indicates the average distance. This indicates the number of neighboring points. , Represents a set The coordinates of the midpoint Represents the coordinates of the center point. It represents the standard deviation.

[0025] Assuming that the set of the center point and its neighboring points follows a Gaussian distribution, the distance from each neighboring point to the center point is compared with the standard deviation. Points whose distance is greater than the standard deviation are considered outliers and are deleted from the original point cloud.

[0026] (2) Point cloud downsampling: Laser point cloud terrain composed of raw laser point cloud data Figure 1The density of the point cloud is generally too high, which is detrimental to the real-time performance of the evaluation algorithm. To improve processing efficiency, it is necessary to reduce the number of point clouds to be processed. The downsampling method uses point cloud voxel filtering. First, the complete 3D point cloud map is segmented, and each segmented point cloud is contained within a voxel with dimensions of [length, width, and height]. In a rectangular voxel, the number of voxels in each direction is:

[0027] in They are respectively The number of voxels divided along the three axes determines the total number of voxel divisions in the point cloud. Individual raster, x max y max z max It is the maximum position coordinate of a point in a point cloud map in three directions, x min y min z min This represents the minimum positional coordinates of a point in a point cloud map in three directions. The average coordinates of all points within each grid cell are calculated, and this average coordinate value is used as the new point to replace all points in the voxel grid. For example... Figure 2 As shown, the number of points in the preprocessed 3D point cloud map is significantly reduced, but the shape remains almost unchanged.

[0028] S3. Perform B-spline surface fitting on the preprocessed 3D point cloud map to obtain the control point array. B-spline curves are combinations of a series of basis curves. They possess excellent properties such as geometric invariance, convex hull property, and local support. The local properties of B-spline curves can be extended to surfaces. The B-spline surface of order is formed by and only by . The overall surface is composed of multiple surface patches, which are determined by a control point and satisfy the continuity between the surface patches.

[0029] Considering both fitting accuracy and speed, a bicubic uniform B-spline surface is used for surface fitting of the laser point cloud. Its expression is as follows:

[0030] In the formula, Indicates in The position is the value of the surface on the z-axis, where This represents the relative position of the target point and the starting coordinates of the current surface patch. Let the vertices of the control lattice of the B-spline surface be denoted by and the basis functions of the cubic uniform B-spline be . and The values ​​are determined by the deBoor-Cox recursive formula, and their specific expressions are as follows:

[0031]

[0032] The expression for a B-spline surface patch in matrix form is:

[0033] in These are the control vertices corresponding to the B-spline surface patch. Expanding the above equation further and substituting it into the specific expression, we get:

[0034] make , , , Ultimately, we have:

[0035] The B-spline surface fitting problem is transformed into a matrix least squares problem, i.e., given a series of point coordinates in a 3D point cloud. ,in , To find the number of points in the point cloud map that participate in the fitting, calculate the control point array. The expression is as follows:

[0036] in, The least squares problem was modeled and solved using Google's open-source solution framework Ceres.

[0037] S4. Using the fitted B-spline surface, based on the control point array... The 3D point cloud map is modeled using B-spline surface modeling to obtain a series of B-spline surface patches. At any position on each surface patch Calculate the gradient of the surface at this location:

[0038]

[0039] in, It is the directional derivative of the surface patch along the x-axis. It is the directional derivative along the y-axis. The gradient at that location is... Expanding and simplifying the above equation, we get:

[0040]

[0041] S5. Calculate the area's traversability cost by combining the mobile robot platform's environmental mobility capabilities.

[0042] Specifically, let ,in Let be any point on the surface. Then the normal vector of any point on the surface can be expressed as: Simplifying, we can obtain the normal vector at any point: .

[0043] Different mobile robot platforms have varying environmental adaptability; the robot's current orientation is determined through a robot localization algorithm. Then the robot is in position. The passability cost at that time can be expressed as:

[0044] in, This is the maximum tilt angle that the mobile robot can safely pass through; on the mobile robot platform used in this invention, this value is 40°. The cost of passage to this location, Let be the angle between the surface normal vector at this position and the robot's orientation, expressed as follows:

[0045]

[0046] in It is an intermediate variable in the calculation process. This represents the vector magnitude. The lower the passage cost, the safer the robot is when passing through the area. The closer the passage cost is to 1, the higher the risk for the robot when passing through the area. When the passage cost equals 1, the robot cannot pass through the area.

[0047] S6. Map the traversability cost of the area to point cloud intensity values ​​to obtain a 3D point cloud map with traversability cost label information. For example... Figure 3 The image shown is a top view of the passable area assessment map, where darker colors indicate greater difficulty in passage. Figure 4 This is the main view of the assessment map of the passable area.

[0048] This invention uses B-spline surface fitting to fit 3D point cloud data, parametrically representing the environment surrounding the mobile robot, and then performs surface smoothing reconstruction of the environment. The gradient of any point on the map is calculated using the fitted parametric surface, and based on this information combined with the mobility of different robots, drivability is evaluated. The parametric surface approach provides a continuous representation of drivability difficulty, ultimately resulting in a point cloud map labeled with drivability costs. This method, based on parametric surface environment representation, can utilize the continuity of the scene and save storage space. Furthermore, the assumptions about the natural environment conditions reasonably simplify the regional drivability cost standard, ensuring computational efficiency in the drivability cost evaluation stage.

[0049] The above description is only a preferred embodiment of the present invention. It should be noted that for those skilled in the art, several improvements and modifications can be made without departing from the principle of the present invention, and these improvements and modifications should also be considered within the scope of protection of the present invention.

Claims

1. A method for passable area evaluation based on three-dimensional point cloud B-spline surface, characterized in that, Includes the following steps: S1. Construct a point cloud map during the robot's movement to obtain the original 3D point cloud map; S2. Perform data preprocessing on the original 3D point cloud map, the data preprocessing including outlier removal and point cloud downsampling; S3. B-spline surface fitting is performed on the three-dimensional point cloud map after data preprocessing to obtain a control point array ; S4. Calculate the gradient at any point on the 3D point cloud map using the fitted B-spline surface; S5. Calculate the traversable cost of the area by combining the environmental mobility of the mobile robot platform; S6. Map the traversability cost of the area to point cloud intensity values ​​to obtain a three-dimensional point cloud map with traversability cost label information; In step S5, the orientation of the current robot pose is obtained by a robot localization algorithm The passable cost of the robot at position is represented as: ， wherein, is the maximum tilt angle that the mobile robot can safely pass through, is the passage cost of the location, is the angle between the surface normal of the location and the robot pose orientation, which is expressed as follows: ， ， wherein, is an intermediate variable of the calculation process, denotes the vector length, denotes the position surface normal vector; The closer the passage cost is to 1, the higher the risk for the robot to pass through the area. When the passage cost is equal to 1, the robot cannot pass through the area. 2.The method of claim 1, wherein, Step S2, "removing outliers," includes: For each point in the original 3D point cloud map, search for N nearest points centered at that point to obtain a set of nearest points; Calculate the average distance and standard deviation from the point to all neighboring points in the set of neighboring points; The distance from each neighboring point to the center point is compared with the standard deviation in turn, and the outliers are deleted from the original 3D point cloud map. 3.The method of claim 2, wherein, Step S2, "point cloud downsampling," includes: segmenting the original three-dimensional point cloud map, each part of point cloud after segmentation is contained in a cuboid voxel with length, width and height of l l , l w , l h ​ The point cloud is divided into L*W*H voxel grids, where: , x max , y max , z max are the maximum position coordinates of the points in the point cloud map in three directions, x min , y min , z min are the minimum position coordinates of the points in the point cloud map in three directions; Calculate the average coordinates of all points within each voxel grid, and replace all points in the voxel grid with the average coordinates as the new point.

4. The method of claim 1, wherein, Step S4 includes: According to the control point array B-spline surface modeling is performed on the three-dimensional point cloud map to obtain a series of B-spline surface patches ; at any position on each curved patch the gradient of the curved surface at that position is calculated.

5. The method of claim 1 to 4, wherein, In step S1, the point cloud map is constructed using the laser SLAM algorithm LIO-mapping during the robot's movement.

Images