Detection method of low raised obstacles and road potholes based on RGB-D depth camera

Abstract

The present invention discloses a method for detecting low, raised obstacles and potholes based on an RGB-D depth camera. The method comprises: preprocessing the original point cloud from the RGB-D depth camera to obtain a corresponding planar point cloud; extracting a raised, low-obstacle point cloud from the planar point cloud; projecting the extracted raised, low-obstacle point cloud onto a planar model parameter plane; downsampling and convex hull segmentation of the raised, low-obstacle point cloud on the model parameter plane; transforming the convex hull to a / map coordinate system and publishing the result; merging the projected raised, low-obstacle point cloud into a ground plane point cloud to form a new planar point cloud; transforming the new planar point cloud to a base_footprint to convert it into a two-dimensional ground plane raster map; performing fake pothole filtering on the two-dimensional ground plane raster map; and extracting potholes using a CV edge extraction method. The present invention provides a method for detecting low, raised obstacles and potholes based on an RGB-D depth camera. The method can simultaneously detect low, raised obstacles and potholes on the ground, thereby improving safety.

Description

Technical Field

[0001] The present invention relates to the field of environmental perception in robot navigation. More specifically, the present invention relates to a method for detecting low raised obstacles and road potholes based on an RGB-D depth camera. Background Art

[0002] During robot operation, the following safety features must be considered: 1. The robot must be able to stop and avoid dynamic and static obstacles; 2. It must be able to prevent falls in areas with low elevations, such as stairways and escalator entrances. These safety features require a robust perception module. Currently, the perception modules of indoor cleaning robots on the market primarily consist of 2D / 3D single- or multi-line lidar, RGB-D depth cameras, ultrasonic radar, and anti-fall sensors. Compared to other sensors, RGB-D depth cameras, due to their high resolution and accuracy, offer significant advantages in detecting low, raised obstacles, potholes, and other road surfaces.

[0003] Current environmental image perception technology cannot simultaneously detect low raised obstacles and potholes on the road during analysis and processing. The detection accuracy is low and it cannot detect low raised obstacles and shallow potholes on the ground. This makes it impossible for small robots to avoid obstacles in time while walking, affecting their walking stability. Summary of the Invention

[0004] An object of the present invention is to solve at least the above problems and / or disadvantages and to provide at least the advantages which will be described hereinafter.

[0005] In order to achieve these objectives and other advantages according to the present invention, a method for detecting low raised obstacles and road potholes based on an RGB-D depth camera is provided, which is characterized by comprising:

[0006] Step 1: Preprocess the original point cloud of the RGB-D depth camera to obtain the corresponding plane point cloud, extract the raised low obstacle point cloud from the plane point cloud, and project the extracted raised low obstacle point cloud onto the plane of the plane model parameters A, B, C, and D;

[0007] Step 2: Downsample the point cloud of raised and low obstacles on the plane of model parameters A, B, C, and D;

[0008] Step 3: Perform convex hull segmentation on the downsampled convex low obstacle point cloud, transform the convex hull into the / map coordinate system and publish it;

[0009] Step 4: Merge the projected raised low obstacle point cloud into the ground plane point cloud to form a new plane point cloud. Transform the new plane point cloud to the base_footprint and convert it into a ground plane 2D grid map.

[0010] Step 5: Fake pothole filtering is performed on the two-dimensional grid map of the ground plane, and the potholes are extracted using the edge extraction method CV.

[0011] Preferably, in the step 1, the preprocessing is configured to include:

[0012] S1, build tf tree;

[0013] S2, establish the model parameters of the static calibration ground plane;

[0014] S3, saves the original point cloud timestamp of the RGB-D depth camera;

[0015] S4, preprocessing the original point cloud;

[0016] S5, performing plane point cloud extraction on the pre-processed original point cloud to obtain plane model parameters;

[0017] S6, comparing the plane model parameters and the model parameters to determine whether the ground plane point cloud is extracted successfully;

[0018] S7, extract the remaining points except the ground plane point cloud from the pre-processed original point cloud, and obtain the raised low obstacle point cloud by filtering the remaining points.

[0019] Preferably, it is characterized in that, in S1, constructing the tf tree includes: setting a timer, publishing a static tf transformation, and constructing a complete tf tree to facilitate the direct transformation of the depth camera point cloud query timestamp into a suitable coordinate system.

[0020] Preferably, in S2, establishing the model parameters for the statically calibrated ground plane includes:

[0021] When the robot is stationary, extract the ground plane point cloud, save the corresponding ground plane model parameters A_, B_, C_, D_, and calculate the distance d_ from the optical center (0,0,0) to the ground plane in the stationary camera coordinate system.

[0022] Preferably, in S3, the original point cloud timestamp of the RGB-D depth camera is saved through a callback function, and the tf transformation relationship of the most recent moment is queried in the tf tree through the point cloud to align the timestamps.

[0023] Preferably, in S4, preprocessing the original point cloud is achieved by filtering out outliers in the original point cloud;

[0024] In S5, after plane point cloud extraction is performed on the preprocessed original point cloud, the extracted plane model parameters A, B, C, D and the plane point cloud are saved, and the distance d from the optical center (0,0,0) to the extracted ground plane in the camera coordinate system is calculated.

[0025] Preferably, in S6, the comparison and judgment method is:

[0026] When the angle between the extracted plane point cloud normal vector and the static calibration ground plane point cloud normal vector is less than the set first threshold, and the absolute value of the difference between the distances d_ and d_ from the two planes to the optical center is less than the set second threshold, the ground plane extraction is considered successful. Otherwise, the plane is re-extracted until the requirements are met.

[0027] Preferably, in S7, when filtering the remaining points, the points on the same side of the plane as the optical center (0,0,0) are retained to obtain a point cloud of raised low obstacles.

[0028] Preferably, in step 5, before performing the pseudo pothole filtering, the projected convex obstacle point cloud is individually transformed to the base_footprint to obtain a convex obstacle two-dimensional grid map;

[0029] The pseudo-pit filtering method includes:

[0030] Perform a simply connected region search on the ground plane two-dimensional grid map to detect all possible pothole areas;

[0031] Perform a single-connected region search on the two-dimensional grid map of raised obstacles and detect all raised obstacle regions;

[0032] Traverse all simply connected areas on the ground plane 2D grid map, and find the connection line from the corresponding pixel position to the base_footprint origin position on the raised obstacle 2D grid map. If the connection line passes through the raised obstacle area, set the pixel value at that position on the ground plane 2D grid map to the default value with point cloud, indicating a pseudo pothole. Otherwise, set it to the default value without point cloud, indicating a true pothole.

[0033] Preferably, the installation height H of the RGB-D depth camera is further limited by the following formula so that the sensing range of the front-facing downward-facing RGB-D depth camera effectively covers the nearby ground area:

[0034]

[0035] Among them, the height of the RGB-D depth camera from the ground;

[0036] β is the downward pitch angle of the RGB-D depth camera;

[0037] α is the vertical field of view of the RGB-D depth camera, ranging from (0° to 90°);

[0038] M is the blind spot of the RGB-D depth camera after tilting downward;

[0039] L is the ground sensing area after the RGB-D depth camera is tilted downward;

[0040] γ is the angle between the RGB-D depth camera’s perception limit and the ground

[0041] The present invention has at least the following beneficial effects: First, the present invention can simultaneously detect low raised obstacles and potholes on the ground, thereby improving safety.

[0042] Secondly, the present invention has high detection accuracy and can detect raised obstacles and potholes with a height and depth greater than 1.5 cm. The robot can accurately avoid the detected raised obstacles and potholes, reducing the off-road capability requirements for the mobile robot.

[0043] Thirdly, the present invention can automatically calibrate the model parameters of the ground plane and the height parameters from the optical center of the camera to the ground when the robot is stationary, thereby facilitating accurate extraction of the ground plane during robot operation.

[0044] Fourthly, the present invention provides a formula for the RGB-D depth camera layout solution. As long as the sensing range on the ground plane is input, the installation height and pitch angle of the RGB-D depth camera can be directly derived.

[0045] Other advantages, objectives and features of the present invention will be reflected in part from the following description and will be understood by those skilled in the art through study and practice of the present invention. BRIEF DESCRIPTION OF THE DRAWINGS

[0046] Figure 1 A schematic diagram of a detection process in one embodiment of the present invention;

[0047] Figure 2 Schematic diagram of calibration of static normal vector and distance of the present invention;

[0048] Figure 3 This is a schematic diagram of the correspondence between the ground surface and the optical center when extracting the point cloud of raised and low obstacles on the ground in the present invention;

[0049] Figure 4 Schematic diagram of calibration of static normal vector and distance of the present invention;

[0050] Figure 5 A two-dimensional grid map of the ground plane of the present invention;

[0051] Figure 6 The two-dimensional grid map of raised obstacles on the ground of the present invention;

[0052] Figure 7 A schematic diagram of the offset of the pothole during point projection in the present invention;

[0053] Figure 8 This is a schematic diagram of the installation of the RGBD depth camera of the present invention in conjunction with the robot;

[0054] Figure 9 A schematic diagram of the process of establishing a TF tree according to the present invention;

[0055] Figure 10 This is a schematic diagram showing the effect of detecting obstacles and segmenting them into a two-dimensional map when the present invention is used for path planning. DETAILED DESCRIPTION

[0056] The present invention will be described in further detail below in conjunction with the accompanying drawings so that those skilled in the art can implement the invention with reference to the description.

[0057] The detection method of the present invention enables small robots to detect low, raised obstacles on the ground and potholes, improving safety. Its high detection accuracy allows it to detect raised obstacles and potholes greater than 1.5 cm in height and depth. This allows the robot to accurately avoid detected raised obstacles and potholes while traveling, reducing the requirements for the mobile robot's off-road capabilities. Furthermore, because the sensor layout is easy to determine and key parameters are readily available, the method is highly convenient.

[0058] The present invention's steps include:

[0059] 1. Build a complete TF tree: Set a timer, publish a static TF transformation from / base_footprint to / camera_link, and build a complete TF tree from / map to / camera_depth_optical_frame: / map-> / odom-> / base_footprint-> / camera_link-> / camera_depth_optical_frame. This complete TF tree facilitates direct transformation of depth camera point cloud query timestamps into the appropriate coordinate system.

[0060] 2. Static calibration of ground plane model parameters: When the robot is stationary, extract the ground plane point cloud and save the ground plane model parameters A_, B_, C_, D_. At the same time, calculate the distance d_ from the optical center (0,0,0) to the ground plane in the camera coordinate system at this time, such as Figure 2 shown.

[0061] 3. Save the original point cloud timestamp of the RGBD depth camera: The callback function saves the original point cloud timestamp, which makes it convenient for the point cloud to query the tf transformation relationship of the most recent moment in the tf tree to achieve the purpose of timestamp alignment.

[0062] 4. Preprocess the original point cloud: filter out outliers in the original point cloud.

[0063] 5. Extract the ground plane from the preprocessed point cloud: save the extracted plane model parameters A, B, C, D and the plane point cloud, and calculate the distance d from the optical center (0,0,0) to the extracted ground plane in the camera coordinate system.

[0064] 6. Determine the extracted plane point cloud: When the angle between the normal vector of the extracted plane point cloud and the normal vector of the statically calibrated ground plane point cloud is less than the set threshold, and the absolute value of the difference between the distances d_ and d_ from the two planes to the optical center is less than the set threshold, the ground plane extraction is considered successful. Otherwise, re-extract the plane until the requirements are met.

[0065] 7. Extract the point cloud of raised low obstacles on the ground: Extract the remaining points except the ground plane from the pre-processed original point cloud, and perform discriminant filtering on the remaining point cloud: retain the points on the same side of the plane as the optical center a (0, 0, 0) (because the optical center and the raised obstacle are both above the ground plane b, such as Figure 3 shown).

[0066] 8. Convert the raised obstacle point cloud from 3D to 2D: Project the raised obstacle point cloud onto the ground plane point cloud of model parameters A, B, C, and D.

[0067] 9. Downsample and convex hull segment the convex low obstacle point cloud after dimensionality reduction: The number of convex low obstacle point clouds after dimensionality reduction is still relatively large, so downsampling is required (downsampling increases the convex hull segmentation rate). The convex hull of the downsampled convex low obstacle point cloud is segmented and the convex hull is transformed to the / map coordinate system and published, such as Figure 4 As shown. (At this point, the detection of raised obstacles is completed)

[0068] 10. Merge the dimensionality-reduced point cloud of the raised low obstacles into the ground plane point cloud to form a new plane point cloud and transform it to base_footprint and convert it into Figure 5 The ground plane 2D grid map: Road potholes are to be extracted using the CV method, so the raised obstacles after dimensionality reduction are merged into the ground plane point cloud (because raised obstacles will leave potholes on the ground plane point cloud, interfering with the actual ground pothole extraction); in addition, the base_footprint is transformed into a 2D grid map for easier processing.

[0069] 11. Transform the convex obstacle point cloud after dimensionality reduction to base_footprint separately and convert it into Figure 62D grid map of raised obstacles: Because raised obstacles on the ground will form a hole area without point cloud on the ground plane point cloud behind them, these hole areas need to be filtered to prevent them from interfering with ground pothole detection.

[0070] 12. Pseudo pothole filtering: Perform a single-connected area search on the ground plane 2D grid map to detect all possible pothole areas; perform a single-connected area search on the raised obstacle 2D grid map to detect all raised obstacle areas c; traverse all single-connected areas on the ground plane 2D grid map, and find a line connecting the corresponding pixel position to the base_footprint origin on the raised obstacle 2D grid map. If the line passes through the raised obstacle area, the pixel value at that position on the ground plane 2D grid map is set to the default value with point cloud (i.e., a pseudo pothole d formed by a raised obstacle); otherwise, it is set to the default value without point cloud (i.e., a true pothole). Figure 5 shown.

[0071] In actual processing, the methods for handling ground potholes and raised obstacles are different. The reason is that: when the point cloud data of raised obstacles is projected onto a plane, there will be no offset, so they can be processed directly; ground potholes are more complicated: 1. The potholes may be deep and there is no returned point cloud data; 2. The pothole e may be an inclined pit, and the returned point cloud will be offset f when projected onto the ground plane. Figure 7 shown.

[0072] Meanwhile, pothole filtering may filter out potholes behind obstacles. This is because potholes behind obstacles meet the filtering criteria and are therefore filtered out. However, potholes that appear before obstacles have a greater impact on wheeled robots. After the robot circumvents the obstacle, it will detect previously undetected potholes, resulting in a minimal impact.

[0073] Sensor selection and layout scheme in this method: The sensing range of the front-facing downward RGBD depth camera can effectively cover the nearby ground area, such as Figure 8 shown.

[0074] H is the height of the RGBD depth camera from the ground;

[0075] β is the downward pitch angle of the RGBD depth camera;

[0076] α is the vertical field of view of the RGBD depth camera, ranging from (0° to 90°);

[0077] M is the blind spot of the RGBD depth camera after tilting downward;

[0078] L is the ground sensing area after the RGBD depth camera is tilted downward;

[0079] γ is the angle between the RGBD depth camera’s perception limit and the ground.

[0080] according to Figure 8 The geometric relationship can be obtained as follows:

[0081] Once the RGBD depth camera's longitudinal field of view angle α, blind spot M, and sensing range L are determined, the camera's pitch angle β and installation height H can be determined:

[0082]

[0083] In the common methods of obstacle detection in the prior art, in order to ensure the accuracy of ground segmentation, the threshold of ground point judgment is increased, which leads to the inability to effectively detect low obstacles. Figure 10 As shown, the method provided by the present invention can overcome this defect, can detect low obstacles and potholes larger than 1.5 cm, and has stronger adaptability.

[0084] The above solution is only an illustration of a preferred embodiment, but is not limited thereto. When implementing the present invention, appropriate replacements and / or modifications can be made according to user needs.

[0085] The number of devices and processing scales described herein are intended to simplify the description of the present invention. Applications, modifications, and variations of the present invention will be readily apparent to those skilled in the art.

[0086] Although the embodiments of the present invention have been disclosed above, they are not limited to the applications listed in the specification and exemplary embodiments. They can be applied to a variety of fields suitable for the present invention. Further modifications will be readily apparent to those skilled in the art. Therefore, the present invention is not limited to the specific details and illustrations shown and described herein without departing from the general concept defined by the claims and their equivalents.

Claims

1. A method for detecting low raised obstacles and road potholes based on an RGB-D depth camera, characterized in that: include: Step 1: Preprocess the original point cloud of the RGB-D depth camera to obtain the corresponding plane point cloud, extract the raised low obstacle point cloud from the plane point cloud, and project the extracted raised low obstacle point cloud onto the plane of the plane model parameters A, B, C, and D; Step 2: Downsample the point cloud of raised and low obstacles on the plane of model parameters A, B, C, and D; Step 3: Perform convex hull segmentation on the downsampled convex low obstacle point cloud, transform the convex hull into the / map coordinate system and publish it; Step 4: Merge the projected raised low obstacle point cloud into the ground plane point cloud to form a new plane point cloud. Transform the new plane point cloud to the base_footprint and convert it into a ground plane 2D grid map. Step 5: Fake pothole filtering is performed on the two-dimensional grid map of the ground plane, and the potholes are extracted using the edge extraction method CV.

2. The method for detecting low raised obstacles and road potholes based on an RGB-D depth camera according to claim 1, wherein: In the step 1, the preprocessing is configured to include: S1, build tf tree; S2, establish the model parameters of the static calibration ground plane; S3, saves the original point cloud timestamp of the RGB-D depth camera; S4, preprocessing the original point cloud; S5, performing plane point cloud extraction on the pre-processed original point cloud to obtain plane model parameters; S6, comparing the plane model parameters and the model parameters to determine whether the ground plane point cloud is extracted successfully; S7, extract the remaining points except the ground plane point cloud from the pre-processed original point cloud, and obtain the raised low obstacle point cloud by filtering the remaining points.

3. The method for detecting low raised obstacles and road potholes based on an RGB-D depth camera according to claim 2, wherein: In S1, building the TF tree includes: setting a timer, publishing a static TF transformation, and building a complete TF tree to facilitate the direct transformation of the depth camera point cloud query timestamp into the appropriate coordinate system.

4. The method for detecting low raised obstacles and road potholes based on an RGB-D depth camera according to claim 2, wherein: In S2, the model parameters for establishing a static calibration ground plane include: When the robot is stationary, extract the ground plane point cloud, save the corresponding ground plane model parameters A_, B_, C_, D_, and calculate the distance d_ from the optical center (0,0,0) to the ground plane in the stationary camera coordinate system.

5. The method for detecting low raised obstacles and road potholes based on an RGB-D depth camera according to claim 2, wherein: In S3, the original point cloud timestamp of the RGB-D depth camera is saved through a callback function, and the tf transformation relationship of the most recent moment is queried in the tf tree through the point cloud to align the timestamps; In S4, the preprocessing of the original point cloud is achieved by filtering out the outliers in the original point cloud.

6. The method for detecting low raised obstacles and road potholes based on an RGB-D depth camera according to claim 2, wherein: In S5, after plane point cloud extraction is performed on the preprocessed original point cloud, the extracted plane model parameters A, B, C, D and the plane point cloud are saved, and the distance d from the optical center (0,0,0) to the extracted ground plane in the camera coordinate system is calculated.

7. The method for detecting low raised obstacles and road potholes based on an RGB-D depth camera according to claim 2, wherein: In S6, the comparison and judgment method is: When the angle between the extracted plane point cloud normal vector and the static calibration ground plane point cloud normal vector is less than the set first threshold, and the absolute value of the difference between the distances d_ and d_ from the two planes to the optical center is less than the set second threshold, the ground plane extraction is considered successful. Otherwise, the plane is re-extracted until the requirements are met.

8. The method for detecting low raised obstacles and road potholes based on an RGB-D depth camera according to claim 2, wherein: In S7, when filtering the remaining points, the points on the same side of the plane as the optical center (0,0,0) are retained to obtain the point cloud of raised low obstacles.

9. The method for detecting low raised obstacles and road potholes based on an RGB-D depth camera according to claim 1, wherein: In step 5, before performing pseudo pothole filtering, the projected convex obstacle point cloud is transformed into base_footprint separately to obtain a 2D grid map of the convex obstacles. The pseudo-pit filtering method includes: Perform a simply connected region search on the ground plane two-dimensional grid map to detect all possible pothole areas; Perform a single-connected region search on the two-dimensional grid map of raised obstacles and detect all raised obstacle regions; Traverse all simply connected areas on the ground plane 2D grid map, and find the connection line from the corresponding pixel position to the base_footprint origin position on the raised obstacle 2D grid map. If the connection line passes through the raised obstacle area, set the pixel value at that position on the ground plane 2D grid map to the default value with point cloud, indicating a pseudo pothole. Otherwise, set it to the default value without point cloud, indicating a true pothole.

10. The method for detecting low raised obstacles and road potholes based on an RGB-D depth camera according to claim 1, wherein: The installation height H of the RGB-D depth camera is also limited by the following formula, so that the perception range of the front-facing downward-facing RGB-D depth camera effectively covers the nearby ground area: Among them, the height of the RGB-D depth camera from the ground; β is the downward pitch angle of the RGB-D depth camera; α is the vertical field of view of the RGB-D depth camera, ranging from (0° to 90°); M is the blind spot of the RGB-D depth camera after tilting downward; L is the ground perception area after the RGB-D depth camera is tilted downward.

Images