Mainpage

Detecting topography of the floor surface for Legged walking Robots Using Microsoft Kinect sensor V2

Supervisors :

Prof. Dr.-Ing. Klaus-Dieter Kuhnert

Dipl.-Inform. Jens Schlemper

Department of Electrical Engineering & Computer Science

Institute of Real-Time Learning Systems (Echtzeit Lernsysteme (EZLS))

Submitted by:

Kerolos Ghobrial

Matr. Nr: 1120651

Abstract

In this project, we propose a scheme to extract the information from Microsoft Kinect V2 in ROS platform. This raw data will be passed on to the Point Cloud Library, which has the state of the art algorithms for preprocessing and processing the data such as, filtering, down sampling, segmentation, clustering and detecting objects. We then demonstrate a real time method to recognize and enhance the features of the ground surface in three different scenarios flat plane, inclined plane and staircase.

The use of the Microsoft Kinect as a sensory input will help the robot to estimate the depth with respect to the camera and the slope between surfaces, where the robot should compensate this angle to avoid falling down, thus be stable while climbing the inclined plane. Another way is to determine the height and width of the stairs then feed this information into the actuators of the robot to walk smoothly and effectively.

Flat and inclined planes:

In the case of flat and inclined planes, the raw point cloud received from the sensor, usually needs some kind of polishing before being handed to a processing algorithms. Using the following steps:

Figure 1: The sequence steps of the inclined plane algorism.

A) Pass through Filter - The original cloud is pass through filter to control the effective volume view, by assigning the proper values to three coordinate x, y and z-axis.
B) Outlier removal: - Filtering the Outliers are lonely points that are spread randomly in the cloud.
C) Voxel grid: - Reducing the complexity of the cloud, by down sampling it. It is an essential step regarding the real time, because fewer points will be processed that are equivalent to the original one.
D) Extracting the planes by two steps: - 1) Region growing: the purpose of this algorithm is to merge the neighboring points that have the same shape pattern by comparing the angles between the point’s normal. 2) RANdom SAmple Consensus (RANSAC): searching the cloud and finding a list of points that support the chosen model, in our case is plane. Moreover, the equations of the planes had been obtained. We apply the region growing algorithm that provides a list of clusters that represent the flat surfaces of the scene, before applying the planar extraction process on each of those clusters. It then computes a correct set of points and a plane model per region cluster.
E) Convex hull: - By using convex hull PCL class, we can obtain the contour of the plane, which is displayed without skipping any frame because the inliers of convex hull is extremely less than segmented plane. This is done by projecting every point of the set on its plane model, and then compute the 2D convex hull of the projected set of points. Furthermore we can get the area of the plane.
F) Reorganized the extracted planes: - After extracting all the planes in the scene such as walls and small undesired planes, which are not organized in the way, we then can calculate the angle of the inclined plane. To do so, the plane of the smallest distance to the camera ”Z axis” will display first and then move up through the far planes. While we are organizing the planes with respect to Z-axis everything else related to this plane such as inliers, coefficients should be sorted as well.
G) Calculate the angle between two planes: - After storing the first three coefficients of the plane, which represent the normal vector in a matrix 3×1, then the angle will be calculated between two planes from the dot product, which is the angle theta between two normal to the planes. if ${A_{1}}X+{B_{1}}X+{C_{1}}Z+{D_{1}}=0$ , and ${A_{2}}X+{B_{2}}X+{C_{2}}Z+{D_{2}}=0$ are the plane equations, then the angle between planes can be calculated by using this formula:
$\small cos\theta =\frac{|{A_{1}}.{A_{2}} + {B_{1}}{B_{2}}+{C_{1}}.{C_{2}}| }{\sqrt{A^{_{1}^{2}}+B_{1}^{2}+C_{1}^{2} } \sqrt{A^{_{2}^{2}}+B_{2}^{2}+C_{2}^{2} } }$
- The Result of flat and inclined plane in real time: - This bar chart diagrams, shows the actual values of slope, and the colored lines shows the error range in a selected slope. The number of samples that had been selected is 15, which the dark blue color represents the positive error in slope and light blue color represents the negative error in slope. Every inclined plane has been chosen randomly. The graph shows, the maximum error occurred in the slope of 50.4 degrees.
Figure 2: Estimated angle vs Real angle in inclined plane.

Staircase:

Real time plane extraction is very important to humanoid robots. In this case, it is crucial to detect multiple organized planes from Kinect v2. In the staircase, there are a lot of planes that should be extracted from the scene very quickly, by using RANSAC for plane segmentation, which is a time-consuming (computationally expensive) to compute for large point clouds and tends to over-simplify complex planar structures, As a result of using RANSAC class multiple the stair steps are often merged into one inclined large plane, which is a huge problem in this case. So we use PCL class pcl::OrganizedMultiPlaneSegmentationgrouping to overcome such a problem and to improve and enhance the way of find the multiple planes. By using this method we can extract a meaningful features, which help us to build 3D models that can be used for autonomous stair climbing for the humanoid robot

Figure 3: The sequence steps of the Staircase algorism.

A) Use Organized Point cloud: - It is an essential step to use the raw data from Kinect v2 without filtering and downsampling (it should be an organized raw point cloud).
B) Filtering the planes according to their centers - By using organized multiple plane segmentation class, the center of the visible planes can be detected according to camera, we can remove undesired planes by assigning the limitation values of the scene in the three axes, which in this case the walls in the front, right and left had been removed.
C) Reoriented the detected planes: - The same as we did in the inclined plane case.
Figure 4: The Real Scene of the stair on the left figure and the stair view in RViz after using our algrothm on the right .
D) Calculate the Width and Height of the stair: -
we used the following formula to determine the distance between two parallel planes :
$Distance = \sqrt{d^{2}_{1} +d^{2}_{2}-(2 \times d_{1} \times d_{2} \times cos \theta ) }$
E) Calculate the Distance from the camera to the first step: -By finding the intersection line between floor and following perpendicular plane to the the floor. by assigning the X-axis to zero ( because the camera detect these planes in a front of it )
-The Result of staircase in real time: - We applied this concept for a real stair, where the camera is far away from the first step by 180 cm and the height of the location of camera with respect to ground is 80 cm (the real height of the real stair is 17.5 cm and the real width is 27 cm). As it declares in the graph, the Random samples with a certain period of time had been taken to calculate the tolerance in the maximum negative and positive error in height and width of the stair, which is showed in the graph with light and dark blue color respectively.
Figure 5: The accuracy of detecting the width and height of the stair in a certain distance to the camera.

Improving the result and the future work

1. Using rqt_reconfigure (formerly dynamic_reconfigure) to set ROS Parameters in running time, which can allow us to see the result immediately, and could be modified to get a perfect result.
2. Using the computer's high power GPU (graphic card) to improve the performance (low computational load) and also CUDA could be used, which can offer dramatic improvement in speed of executing our algorithm.

Conclusions

The overall objective of this work is to use a point cloud library to discover the topology of the planes for three different scenarios - the flat surface, inclined surface and the staircase.

The initial implementation used the RANSAC PCL class to find the planes. It was observed that by using this class only, a few numbers of planes could be detected and it can be used only in case of flat and inclined planes. To overcome this problem, the Organized Multi-Plane Segmentation class is used to help detecting the stair steps. Along with this it also can solve the problem of plane merging that was observed using RANSAC. The computation time needed for this class is also comparatively lower than that achieved using RANSAC.
After detecting the useful planes of the scene, they are sorted according to the one, nearest to the camera, after that the angle between two sequence planes is calculated (in case of the Flat and incline plane), but (in the staircase) a specific condition is applied, where it should detect the floor that contains much more density points and the angles between steps of the stair should be within the range. When all these conditions occur, the algorithm starts to calculate the height, width and the distance between the camera and first step. All this information will be provided to the humanoid robot to compensate the angle of inclined plane (balance state), to prevent it from colliding and to help to follow the path.

Aktualisiert via XIMS am 9.6.2017 von M. Langer