Humanoid robot field continuous slope terrain recognition method

Abstract

The invention discloses a humanoid robot field continuous slope terrain recognition method. The method comprises the following steps: S1, acquiring point cloud data of an environment by using a laser radar; s2, performing RASG rasterization on the point cloud data, and dividing continuous point cloud data into a plurality of region blocks; s3, traversing each region block, and converting the point cloud data in each region block into elevation information to form an elevation grid; s4, calculating a slope grid according to the elevation grid; and S5, clustering the gradient grids, and grouping similar gradient regions, the terrain can be divided into different continuous gradient regions, the features of the terrain are clearer and easier to analyze, the variable-size radial grids are adopted to segment the point cloud data, adaptive processing can be performed according to point cloud densities at different distances, and the accuracy of the point cloud data is improved. And the calculation complexity is greatly reduced.

Description

Technical Field

[0001] The invention belongs to the field of humanoid robot patrol navigation, and in particular relates to a method for identifying field continuous slope terrain of a humanoid robot. Background Art

[0002] With the continuous advancement of humanoid robot technology, military and civilian patrol missions in the field are showing a diversified trend. Future field exploration missions will continue to pursue higher scientific returns under the premise of fully guaranteed reliability. Therefore, using the limited lifespan of humanoid robots to autonomously explore more scientifically valuable landforms and landforms has become a key factor in improving the scientific returns of field patrol missions. Although autonomous landform perception technology can effectively improve detection efficiency, for reliability reasons, current field patrol missions are still mainly semi-autonomous remote operation, and autonomous perception technology for field landforms is still in the conceptual research and preliminary simulation test stage. Summary of the invention

[0003] The purpose of the present invention is to provide a method for identifying continuous slope terrain in the field by a humanoid robot, which can realize the robust identification of the continuous slope terrain in the detection area by the patroller and reduce the time and space complexity of the system identification calculation.

[0004] In order to solve the above technical problems, the technical solution adopted by the present invention is: a method for identifying continuous slope terrain in the field by a humanoid robot, comprising the following steps:

[0005] Step S1: Use laser radar to obtain point cloud data of the environment;

[0006] Step S2: performing RASG rasterization on the point cloud data, and dividing the continuous point cloud data into a plurality of area blocks;

[0007] RASG rasterization is radially adaptable sector grid rasterization. First, the scene is divided into different areas. Different colors represent different areas, and the grid sizes are also different. m Represents the mth region in RASG Total N Z Then, in each region Z m The radial and tangential directions are further divided into small grids, the number of which is N. r，m ×N θ，m , defined as:

[0008]

[0009] in:

[0010]

[0011] L represents the radial distance from the radar, and the subscripts of L are L values ​​with different meanings. This step converts the point cloud data into a processable grid form, providing a basis for the subsequent calculation of the grid slope.

[0012] Step S3: traverse each of the area blocks, convert the point cloud data in each of the area blocks into elevation information, and form an elevation grid; specifically, map the point cloud data in each of the area blocks to the corresponding grid grid, select the z coordinate of the lowest point in each of the grid grids as the elevation value in the grid grid, and convert the point cloud data into an elevation grid to obtain the vertical change information of the terrain. Then, by calculating the slope of the elevation grid, the slope characteristics of the terrain can be obtained, wherein the elevation grid is calculated as follows: first, map the point cloud data in the area to the corresponding grid grid. The laser radar can measure the x, y, and z coordinate values ​​of each point. For each grid, select the coordinates of the lowest point as the elevation value of the grid. In this way, an elevation grid is obtained, in which the value of each grid represents the elevation of the lowest point in the area.

[0013] Step S4: Calculate the slope grid according to the elevation grid.

[0014] Step S5: clustering the slope grids to group similar slope areas. In each slope area, the slope grids are grouped into different clusters, and each cluster contains a group of adjacent grids.

[0015] Step S6: Projecting the slope information onto the two-dimensional image.

[0016] Furthermore, in the step S1, 3D point cloud data of the real-time scene in front is acquired by the laser radar, with the field of view facing forward.

[0017] Furthermore, in the step S2, radial variable-size fan-shaped grids are used for gridding, and each of the fan-shaped grids contains a certain range of the point cloud data.

[0018] Furthermore, in step S4, the elevation difference between each elevation grid and its adjacent elevation grid is calculated, and the elevation difference is converted into a slope value.

[0019] Furthermore, the average value of the slope values ​​is used as the slope value of the slope grid.

[0020] In summary, due to the adoption of the above technical solution, the beneficial effects of the present invention are:

[0021] The method proposed in the present invention can divide the terrain into different continuous slope areas, making the characteristics of the terrain clearer and easier to analyze. The point cloud data is segmented by using radial grids of variable sizes, and can be processed adaptively according to the point cloud density at different distances, greatly reducing the computational complexity. BRIEF DESCRIPTION OF THE DRAWINGS

[0022] In order to facilitate understanding by those skilled in the art, the present invention is further described below with reference to the accompanying drawings.

[0023] Figure 1 The present invention provides a flow chart of a method for identifying continuous slope terrain in the field by a humanoid robot.

[0024] Figure 2 Schematic diagram of the radially variable-size fan-shaped grid provided by the present invention.

[0025] Figure 3 A schematic diagram of an elevation grid is provided for obtaining the present invention.

[0026] Figure 4 This is a schematic diagram of the calculation grid slope provided by the present invention.

[0027] Figure 5 A schematic diagram of converting the 3D Bounding Box provided by the present invention into 2D;

[0028] Figure 6 Schematic diagram of the camera viewing angle and input point cloud provided by the present invention. DETAILED DESCRIPTION

[0029] The technical solution of the present invention will be described clearly and completely in conjunction with the embodiments below. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. The embodiments of the present invention and all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0030] LiDAR uses traditional methods and deep learning methods. For the slope, the traditional method considers selecting the area of ​​interest in front of the vehicle, using the RANSAC algorithm to fit the plane of these areas and calculate their normal vectors, and using the IMU information as a benchmark to obtain the slope value of the area of ​​interest. Considering that the beam of LiDAR, especially the long-distance beam, is sparse, the grid between the laser lines of the same obstacle will be judged as an unknown state area because there is no laser point. Here, the connected area labeling method is used to cluster obstacles. Due to the rapid development of deep learning, many end-to-end semantic segmentation models have been produced at this stage, including semantic segmentation of images and semantic segmentation of LiDAR, such as gscnn and GA-Nav models for images, and SalsaNet and SalsaNext for LiDAR. After data learning, these models output obstacles and drivable areas in images or lasers in an end-to-end manner. These models have achieved good results in the SemanticKITTI dataset, and have also achieved very good results in migrating to the RELLIS-3D off-road dataset, so these end-to-end semantic segmentation models can be considered.

[0031] The binocular camera uses a deep learning method and uses the U-NET model or the Salsanext model for image segmentation. The three-dimensional information of the segmentation result is obtained according to the depth map. This method can achieve pixel-level usable area extraction results.

[0032] Therefore, the use of LiDAR can realize the measurement of horizontal plane angles in a 3D environment. The continuous slope area detection of this patent is based on a radial grid of variable size and a clustering algorithm, so the accuracy required for each slope elevation calculation can vary according to the distance. Finally, different amounts of point cloud data are used for processing, which greatly reduces the time complexity and space complexity of the algorithm.

[0033] Example 1

[0034] like Figure 1 As shown, the present invention provides a method for identifying continuous slope terrain in the field by a humanoid robot, which uses point cloud for continuous slope terrain identification, with LiDAR point cloud (laser radar point cloud) as input and continuous slope terrain identification result as output. First, the LiDAR point cloud is RASG-rasterized to divide the continuous point cloud data into multiple area blocks ( Figure 2In the figure, different colors represent different area blocks), which helps to segment the point cloud data to obtain more detailed terrain features. Then, for each area block, traverse to calculate the elevation information of the grids in it. By analyzing the point cloud data in the area, the elevation distribution of the grids in the area can be obtained. Then, based on the calculated elevation grid (there are several grids in each area block, and the grid sizes in different area blocks are different), the slope grid is further calculated. The slope is an important terrain feature, which can be obtained by calculating the elevation change of each grid point. In this way, the slope information at different locations in the area can be obtained. Finally, the slope grids are clustered to group similar slope areas. This can divide the terrain into different continuous slope areas, making the characteristics of the terrain clearer and easier to analyze.

[0035] The present invention specifically introduces a method for identifying continuous slope terrain in the field by a humanoid robot, which includes the following steps:

[0036] Step S1: Use a laser radar to obtain point cloud data of the environment; obtain 3D point cloud data of the real-time scene in front through the laser radar, with the field of view facing forward.

[0037] Step S2: RASG rasterize the point cloud data, dividing the continuous point cloud data into a number of area blocks; using radial variable size fan-shaped grid rasterization, each of the fan-shaped grids contains a certain range of the point cloud data.

[0038] RASG rasterization is radially adaptable sector grid rasterization. First, the scene is divided into different areas. like Figure 2 As shown in the figure, different colors represent different areas, and the grid sizes are also different. m Represents the mth region in RASG Total N Z areas, which is set to 4 in this patent.

[0039] Then, in each region Z m The radial and tangential directions are further divided into small grids, the number of which is N. r，m ×N θ，m , defined as:

[0040]

[0041] in

[0042]

[0043] L represents the radial distance from the radar. The subscript of L represents the L value with different meanings.

[0044] This step converts the point cloud data into a processable mesh form, providing a basis for the subsequent calculation of the mesh slope.

[0045] Step S3: traverse each of the area blocks, convert the point cloud data in each of the area blocks into elevation information, and form an elevation grid; specifically, map the point cloud data in each of the area blocks to the corresponding grid grid, select the z coordinate of the lowest point in each of the grid grids as the elevation value in the grid grid, and convert the point cloud data into an elevation grid to obtain the vertical change information of the terrain. Then, by calculating the slope of the elevation grid, the slope characteristics of the terrain can be obtained.

[0046] Elevation grid calculation: First, map the point cloud data in the area to the corresponding grid. The laser radar can measure the x, y, and z coordinates of each point. Figure 3 As shown, for each grid, the z coordinate of the lowest point is selected as the elevation value of the grid, thus obtaining an elevation grid in which the value of each grid represents the elevation of the lowest point in the area.

[0047] Step S4: Calculate the slope grid according to the elevation grid; specifically, calculate the elevation difference between each elevation grid and its adjacent elevation grid, and convert the elevation difference into a slope value; and use the average value of the slope values ​​as the slope value of the slope grid.

[0048] Slope calculation: Using the elevation grid, the slope of each grid can be calculated. For each grid, the elevation difference between it and the adjacent grid is calculated, and then these elevation differences are converted into slope values. Figure 4 For example, Figure 4 There are 9 grids (such as Figure 4 ), calculate the elevation difference between the center grid and the other 8 grids surrounding it (as shown in (a) of Figure 4 The elevation difference is divided by the horizontal distance between adjacent grids to obtain the tan value, and the arctan is the slope value (as shown in (b)). Figure 4 In order to more accurately represent the slope of the terrain, the slope change between a grid and its adjacent grids is usually calculated, and then these slopes are averaged to obtain the slope value of the grid. Figure 4 As shown, by traversing all grids, the slope grid of the entire area can be obtained.

[0049] Step S5: clustering the slope grids to group similar slope areas; in each slope area, the slope grids are grouped into different clusters, each cluster containing a group of adjacent grids.

[0050] First, the slope grid is screened and defined according to the terrain characteristics and patent requirements. In this patent, a slope of 5 to 10 degrees is defined as a gentle slope, and a slope of 10 to 20 degrees is defined as a steep slope. Figure 4 In (c), the numbers in the 9 grids are slope values ​​in rad. This definition helps to divide the terrain into different slopes, which is convenient for subsequent analysis and identification.

[0051] Next, clustering is performed separately for gentle slopes and steep slopes to identify continuous slope regions. For each slope region, the grids are grouped into different clusters, each cluster containing a set of adjacent grids whose slopes are within a specific range. Such clusters represent continuous slope regions on the terrain. If there is an empty area between adjacent grid regions after clustering, two clusters are formed.

[0052] In order to organize the slope clusters into continuous slope areas, a cluster size threshold needs to be set. Only when the number of grids in the cluster is within the threshold range, it is considered to be a continuous slope area. In this patent, the thresholds set for each area from near to far are 20-50, 30-60, 30-60 and 10-30 respectively.

[0053] Step S6: Projecting the slope information onto the two-dimensional image.

[0054] In order to more intuitively display the detected continuous slope terrain, it is necessary to project the slope information onto a two-dimensional image and convert the three-dimensional bounding box (3D Bounding Box) into a two-dimensional bounding box (2D Bounding Box), such as Figure 5 shown.

[0055] Projection to 2D image: In order to display the slope area on the image, the location information of the slope needs to be projected from the LiDAR coordinate system to the pixel coordinate system of the image. This can be achieved through geometric calculations, considering the camera's projection model, and mapping the 3D coordinates of the obstacle to the 2D coordinates on the image.

[0056] 3D Bounding Box to 2D Bounding Box: For each detected obstacle, a 3D bounding box is usually used to represent its spatial range. In order to display it on the image, these 3D bounding boxes need to be converted to 2D bounding boxes. This can be done by projecting each corner point of the 3D bounding box onto the image and then taking the minimum enclosing rectangle of the projected points to obtain the 2D bounding box.

[0057] The preferred embodiments of the present invention disclosed above are only used to help explain the present invention. The preferred embodiments do not describe all the details in detail, nor do they limit the invention to only specific implementation methods. Obviously, many modifications and changes can be made according to the content of this specification. This specification selects and specifically describes these embodiments in order to better explain the principles and practical applications of the present invention, so that those skilled in the art can understand and use the present invention well. The present invention is limited only by the claims and their full scope and equivalents.

Claims

1. A method for identifying continuous slope terrain in the field by a humanoid robot, characterized in that: The following steps are involved: Step S1: Use laser radar to obtain point cloud data of the environment; Step S2: performing RASG rasterization on the point cloud data, and dividing the continuous point cloud data into a plurality of area blocks; RASG rasterization is radially adaptable sector grid rasterization. First, the scene is divided into different areas. Different colors represent different areas, and the grid sizes are also different. m Represents the mth region in RASG Total N Z Then, in each region Z m The radial and tangential directions are further divided into small grids, the number of which is N. r，m ×N θ，m , defined as: in: ΔL m =L max，m -L min，m ,L max，m =L min，m+1 ; L represents the radial distance from the radar, and the subscripts of L are L values ​​with different meanings. This step converts the point cloud data into a processable grid form, providing a basis for the subsequent calculation of the grid slope. Step S3: traverse each of the area blocks, convert the point cloud data in each of the area blocks into elevation information, and form an elevation grid; specifically, map the point cloud data in each of the area blocks to the corresponding grid grid, select the z coordinate of the lowest point in each of the grid grids as the elevation value in the grid grid, and convert the point cloud data into an elevation grid to obtain the vertical change information of the terrain. Then, by calculating the slope of the elevation grid, the slope characteristics of the terrain can be obtained, wherein the elevation grid is calculated as follows: first, map the point cloud data in the area to the corresponding grid grid. The laser radar can measure the x, v, and z coordinate values ​​of each point. For each grid, select the coordinates of the lowest point as the elevation value of the grid. In this way, an elevation grid is obtained, in which the value of each grid represents the elevation of the lowest point in the area. Step S4: Calculate the slope grid according to the elevation grid. Step S5: clustering the slope grids to group similar slope areas. In each slope area, the slope grids are grouped into different clusters, and each cluster contains a group of adjacent grids. Step S6: Projecting the slope information onto the two-dimensional image.

2. A method for identifying continuous slope terrain in the field by a humanoid robot according to claim 1, characterized in that: In the step S1, the 3D point cloud data of the real-time scene in front is acquired by the laser radar, with the field of view facing forward.

3. The method for identifying continuous slope terrain in the field by a humanoid robot according to claim 1, characterized in that: In the step S2, radial variable-size fan-shaped grids are used for gridding, and each of the fan-shaped grids contains a certain range of the point cloud data.

4. The method for identifying continuous slope terrain in the field by a humanoid robot according to claim 1, characterized in that: In step S4, the elevation difference between each elevation grid and its adjacent elevation grid is calculated, and the elevation difference is converted into a slope value.

5. The method for identifying continuous slope terrain in the field by a humanoid robot according to claim 4, characterized in that: The average value of the slope values ​​is used as the slope value of the slope grid.

Images