Stereo vision-based obstacle avoidance module on 3D point cloud data

ABSTRACT


INTRODUCTION
Recent development on autonomous platform geared towards assisting human in their daily activity mandates all autonomous platform to have a capability to perceive the surrounding environment, and make a suitable decision for action.One essential action is to safely navigate cluttered environment and avoiding collision with any obstacle that presents within the environment.One challenge present is avoiding collision with obstacle and navigating in an unstructured environment [1].Since unstructured environment have so many possibilities of obstacle position, orientation and or shape, challenge present involved in gaining obstacle position, orientation and size in order to safely plan a path to avoid a collision.One commonly used solution to deal with these problems is 3D vision [2,3] based solution to gain obstacle position, orientation and size since with 3D vision the system did not need to recognize and learn the object to acquire the information on object position, orientation and size since the raw data itself is already 3D.Many solutions exist on the field of gaining 3D information from the environment, one of the solution is using computer stereo vision [4][5][6] to  Stereo vision-based obstacle avoidance module on 3D point cloud data (Eko Purbo Wahyono) 1515 create an point cloud data [7].On solving the problems on 3D vision and obstacle avoidance, as the system become progressively more complex and demanding with processing power, the demands for more details on input on environment and faster execution time become more clear.From the input processing side, with 3D vision that create a 3D representation of surrounding environment with great details comes with the price of enormous data size and demand more processing power to process the data.One solution to this problem is to reduce data size while keeping the characteristics, normally this result in reduced environment details.RANSAC (random sample consensus) based method with SAC model [8] is one of the most used method used to reduce data size without sacrificing too much details on environment data and robust in presence of noise [9], This work uses NaN, passthrough and voxel grid filter based on RANSAC to reduce data size.To further reduce computation, parallel computing are used [9][10][11].Based from [9] comparison this work used CUDA (compute unified device architecture) parallel computing based on [12].The reason for parallel computing is to minimize execution time in order to enable the obstacle avoidance module to be in a processing unit combined with another function for platform movement and automation.
Environment data must be classified into regions with similar characteristic to facilitate further processing.One of data classifying is data segmentation.This process was required to differentiate data to acquire usable environment data [13][14][15][16].This work using plane-based segmentation to differentiate between obstacle which defined as any point cloud data with vertical plane orientation and the horizontal walking plane itself.This step is used to segment the walking plane and obstacle data.Obstacle data, a representation of environment condition are used to plan the collisionfree path.Since the platform movement lies on x,y axis, the obstacle data in form of point cloud can be converted to grid map [17,18].Recent research produce an algorithm that not only able to plan safe path but also deal with environment change and platform disturbances that cause the platform to stray from planned path.One of the algorithm is timed-elastic-band [19].Many research has been done on improving TEB (timed elastic band) method [20][21][22][23][24], this work base the TEB application from research by [20,22].The final goal of this work is a system module with integrated processing for 3D point cloud data similar to [25] with an output of obstacle detection and obstacle position information to produce path planning with an ability of updating the path to deal with environment changes and or platform deviation while record the environment data for future usage and subsequent global path planning.

PROPOSED SYSTEM OVERVIEW
Below are the general overview of steps taken to process environment data to plan the obstacle avoidance action: − Pre-Processing.Raw point cloud data from a stereo camera are filtered to down sample the data using voxel grid method and remove unnecessary points.− Obstacle extraction.Environment data in form of point cloud are separated between vertical and horizontal plane based on point data orientation, environment interpreted into a grid to get the information of safe moving space and obstacle area, the resultant grid are recorded for future use and global navigation.− Path planning.Plan a path towards goal through safe moving area using occupancy map, keeping clear of obstacles area while updating environment grid and adapt the path when environment data and platform position are changed, publish array of waypoints along the global path and create new waypoints array towards global path target.

RESEARCH METHOD 3.1. Point cloud engine
On camera initialization, camera calibration and pre-set parameters are loaded to enable image acquisition.Data taken from Stereo 2D camera with focal point  and baseline  are stereo matched, and triangulated to gain depth data by: where   ,   and   ,   are matching pixel between left and right camera respectively, the resultant depth data is combined with RGB data from the camera and converted into 3D point cloud data format by normalizing the pixel coordinate (, , ) into 3D coordinate (, , ) with camera baseline  using: to be processed afterwards by subsequent sub-systems.Point cloud resultant of 1 and 2 have parallel planes in real condition shown as parallel planes, therefore eliminating the need of calculating camera perspective.All these processes are done in an early stage inside the camera itself.

Filtering
Since dense point cloud data while provide more details on environment leads to longer execution time needed for processing, therefore a method for keeping environment details while reducing point count that leads to faster execution is needed [26].This system uses voxel grid filtering which group data with similar characteristics and replace the group a single data that represent the whole group, this filter was chosen for the robust characteristic of the filter.Point cloud data is down-sampled using voxel grid method.A defined Oriented bounding box  along with the minimum and maximum value of point cloud in , ,  axis are used to calculate voxel grid   in size of   ×   ×   which defined as: Defined size of a voxel is used to group neighbouring point of an initial point Pi with similar characteristics and using the Pi as centroid all other point removed leaving the centroid point data afterwards.Small voxel size, while keep more details of raw point cloud data, leads to longer execution time while bigger voxel size have smaller execution time but loses more details.Resultant of this step are as shown on Figure 1 while voxel grid is slower, it represents the underlying surface more accurately without sacrificing keypoint of 3D point cloud data [27].This characteristic will enable the segmentation process to segment the data properly.The resultant data from this step still contains all data within camera's field of view, since the system do not need roof information and existence of roof data in occupancy grid map process can lead to no available moving space, hence the removal of roof point cloud data is needed.The system use a passthrough filter with maximum vertical parameters to exclude roof point cloud [27], if passthrough filter is not exist, the roof segmented will be defined as an obstacle since the roof plane itself lies parallel with walking plane, this happened because the segmentation process only separate vertical and horizontal point cloud data orientation.Because the roof will not hinder platform movement and including roof as obstacles result in no free obstacle area, the system exclude ceiling point cloud data from being processed by the segmentation process by filter the data using passthrough method: with P as the resultant point cloud,  as an individual point, m as the minimum z axis value and n as maximum z axis value.

Segmentation
After pre-processing step, the point cloud data segmented to gain information on walking plane as free moving region and obstacle data.With the definition of walking plane as a plane in ,  axis.By defining the plane's mathematical model as: where , ,  on (8) are the parameters of the mathematical model of the plane and k,l,m are the independent variables of the model to define a plane.To determine if point  = ( 0 ,  0 ,  0 ) fit plane  =  ×  +  ×  +  ×  + , the perpendicular distance of the point to the plane was calculated using: If the point is below the threshold value, it can be considered as an inlier to the plane.By using (7) to define the walking plane model and isolate the walking plane point cloud data from others using (8) resulted in separation of the plane inlier from other point, the system has gained both free region in form of inlier points and the obstacle point cloud data itself to plan the obstacle avoidance path.Result of this step is shown by Figure 2.

Occupancy grid map
Segmented point cloud data are used for building occupancy grid map to acquire obstacle position relative to the platform and the subsequent map can be recorded for future use or for global path planning to already recorded area.Occupancy grid map created using obstacle point cloud data to determine if a grid occupied by an obstacle that will cause collision [18].Each cell Ci = x, y with a fixed size for each point Pi = x, y, z This step gains areas where the platform can safely move without risk of collision without the need of single out every obstacle and calculate the centroid and position of each obstacle by measuring each area and comparing the width with the minimum width required by the platform.The result as shown on Figure 3.

Path planning
This part of the system have two parallel process, the first one, path planner create a global solution for the platform to move towards the target.The second system handle disturbance force on a platform and output correction command to deal with small changes in environment and unexpected obstacle.Based on timed-elastic-band method by Quinlan [19] with development on sparse model [21] to advanced trajectories [20] with MPC approach towards TEB methods [22].Overview of this step is as shhown on Figure 4.This method was used because the behavior of the platform is not determined at the planning level, yet local disturbances will not hinder the system ability to reach goals.This system deals with local disturbances by deforming the path when environment changes are detected.Despite this step, the system still maintain a complete collision-free path towards a determined goal.Find the shortest path towards determined goals.With (10) produces global path result that used by TEB to create a series of waypoints.The shortest path, taken as a series of a fixed number  of waypoint   with tolerance radius  which comprise.
Array of waypoint  can be denoted as: TELKOMNIKA Telecommun Comput El Control


Stereo vision-based obstacle avoidance module on 3D point cloud data (Eko Purbo Wahyono)

1519
This system does not pursue fastest trajectory and in a context of timed elastic band, it only optimizes the location of intermediate waypoint as the system does not have a boundary of final platform state, therefore the change in time for each consecutive waypoint (∆  ) are fixed and can be denoted as (13).
∶= (, ) Optimal band calculation is calculated by minimizing the objective function.
With an array of waypoints Pi from path planning result, the system compare current platform position and orientation information with waypoints coordinate and update waypoints array when the system achieve waypoints or if the system stray from global path and previous waypoint array.

RESULTS AND ANALYSIS
The result of voxel grid filtering with 0.05 m size voxel, the resultant cloud shows a drop of 98.2% in point cloud count from 921600 point to 16664 point.With only 1.8% of the data remains while maintains similar characteristics with the raw data.This result shows that voxel grid preserved underlying plane of the raw point cloud data while reducing point cloud data size as shown on Figure 1.Filtering sub-system takes 86 ms average execution time with 720p raw data size with an acceptable result for segmentation while reducing the data size and removal of unnecessary data proven by the success on segmentation step as shown on 2a, faster execution time leads to more loss of details, which result in failure of segmentation.Execution time for filtering subsystem can be improved by the usage of parallel computing for each filtering model for faster execution.Similar method can be applied for segmentation subsystem which takes 260 ms average execution time.Shorter execution time leads to faster map update and faster decision result.Segmentation resulted in all point cloud data with vertical plane orientation are separated from the point cloud data with horizontal plane orientation.Compared to colour based point cloud clustering, the result shows that the colour based segmentation result are still detecting some vertical object as a same group as the horizontal walking plane, while the system's segmentation based on each point orientation separate all points with vertical plane orientation.Despite system success of segmentation between horizontal walking plane and obstacle data, the system fail to process some data when stereo matching failure exist, this characteristics make neighbouring pixel with a significant distance in 3D environment result in an almost horizontal gradient, therefore the system segmentation does not segment this area.The comparison between plane based segmentation and colour based segmentation are shown on Figure 2 occupancy grid map resulted in information of each obstacle data position therefore safe moving area are acquired where no obstacle are present.
The system will calculate the safe area around the obstacle grid to account for platform size and odometry drift.All these information are successfully recorded proven by the ability of the system to plan navigation path through already mapped area Figure 3 show that where obstacle point cloud exist, the corresponding cells show as black cells with no failure for all obstacle point information.Path planning resulted the shortest path in regard to platform size to avoid collision with obstacle area and modify initial path to target to avoid an obstacle and adapt to new environment data this is proven by moving the camera according to path planning resulted in arriving to target coordinate without any collision shown on Figure 5 (a) and Figure 5 (b) shows the same goal position with updated environment information and platform position.Figure 5 (b) shows the system update the path plan towards the goal when the platform is moved outside the planned path and new obstacle information is acquired from the environment and update path when the platform moved inside planned path Figure 5 (c).The system only produce goal array position as long as the area width for the path planning are wider than the required width of the platform and will terminate the path planning when the required condition are not met, the system will produce path as close as the target as possible when the condition are met, shown on Figure 5 (d), farthest arrow's position have area width of more than 0.7 m, or shown at the Figure 5 (d) as the blue circle diameter are 7 grid with each grid have the width of 0.1 m, while further possible position did not met this requirement, therefore the system are fulfilling the target of creating safe path without possible collision.

CONCLUSION
Filtering sub-system perform amicably with total execution time of 96ms on average.This result of execution time is done on 720p raw point cloud data size with 0.05m voxel size and limit passthrough of 1.2m above ground.All sub-sub-system of filtering step perform as intended.Obstacle Extraction sub-system extract all obstacle data and mapped with appropriate result compared to the real environment.While segmentation process have a point of failure when the resultant of stereo matching did not produce the same result as the real condition which result in segmentation failure while occupancy grid map produces the exact result of every segmented obstacle data.
Path planning sub-system produces appropriate path and correction command when dealing with environment and platform update while keeping the recorded data for future use and for global navigation.This system did not deal with global path planning with previously mapped environment and has no superposition odometry present.This system has provided a capability of obstacle avoidance based on 3D point cloud data interpreted by equation 10 to form basic intelligence based on TEB model to deal with collisionfree path planning and deals with platform deviation.

Figure 5 .
Figure 5. Obstacle avoidance path modification, (a) initial path data, (b) after environment and position update (c) After position update, (d) Minimum width requirement not met with the voxel grid size defined, assign each grid to each point   in point cloud .After each grid   assigned and the center gravity point   for each grid   with   ,   ,   and   ,   ,   as the outmost point of the   grid calculated by: in voxel are replaced by center gravity point of each grid   with final position point set ´. Process all voxel grid to acquire ´ by: