Sensor Fusion of Laser & Stereo Vision Camera for Depth Estimation and Obstacle Avoidance

Transcription

1 Volume 1 No. 26 Sensor Fusion of Laser & Stereo Vision Camera for Depth Estimation and Obstacle Avoidance Saurav Kumar Daya Gupta Sakshi Yadav Computer Engg. Deptt. HOD, Computer Engg. Deptt. Student, Electrical & Electronics Delhi Technological University Delhi Technological University Delhi Technological University ABSTRACT Laser Range Finders (LRF) have been widely used in the field of robotics to generate very accurate 2-D maps of environment perceived by Autonomous Mobile Robot. Stereo Vision devices on the other hand provide 3-D view of the surroundings with a range far much than of a LRF but at the tradeoff of accuracy. This paper demonstrates a technique of sensor fusion of information obtained from LRF and Stereovision camera systems to extract the accuracy and range of independents systems respectively. Pruning of the 3D point cloud obtained by the Stereo Vision Camera is done to achieve computational efficiency in real time environment, after which the point cloud model is scaled down to a 2-D vision map, to further reduce computational costs. The 2D map of the camera is fused with the 2D cost map of the LRF to generate a 2-D navigation map of the surroundings which in turn is passed as an occupancy grid to VFH+ for obstacle avoidance and path-planning. This technique has been successfully tested on Lakshya - an IGV platform developed at Delhi College of Engineering in outdoor environments. Keywords Sensor fusion, Stereovision, Laser range finder, Obstacle avoidance, Navigation map, 3D point cloud and Robotics 1. INTRODUCTION There are a large number of sensors available which can be used to detect obstacles present in the immediate surroundings, for eg. sensors like sonar, lasers, stereo vision camera, etc are widely used for obstacle detection. Each sensor works in a different manner and has its own limitation and advantages. Due to its inherent limitations, a single sensor cannot give an accurate reconstruction of the surroundings and hence cannot be used by mobile robots for obstacle detection and accurate path planning. This gives rise to the concept of sensor fusion i.e. integration of data from different sensors for successful obstacle avoidance and path planning. Distance sensors like laser range finders have been used before reconstruction of real world surroundings of a robot [1]. They give very accurate and reliable output, but in case of obstacles like a chair or table or obstacles not lying in the plane of the laser, they fail to detect the whole obstacle. Also, laser data is very much affected by the pitch and roll of the vehicle. On the other hand, stereo vision camera is involved in the acquisition of images of the dynamic environment. Though it can perceive up to infinity, its field of view is narrower as compared to that of LRF s. Also, if only the camera system is used for obstacle detection the data obtained is inferior in quality and it increases the computation burden on the system. Sensor fusion with Laser and Camera has been accomplished before in [2] but the method focuses on generating 3-D maps of 2D Laser maps and then fusing it with stereovision 3D map, which adds to computational burden. [3] deals with long range obstacle detection on road for which laser range finder detects and tracks the obstacle and stereovision camera system reconfirms the laser data. Sensor fusion of sensors like stereovision and lidar systems have been used widely for autonomous vehicles [4] [5]. In this paper, we propose an algorithm which relies on the fusion of the 2D cost maps generated by laser data with the 2D cost maps generated from the 3D real world map by stereo vision camera systems, to create an Occupancy grid for obstacle detection and trajectory planning. The fusion of both is a challenging task but the output is commendable and quite efficient to make a system move autonomously in a complex, dynamic environment with safe path planning and obstacle collision avoidance. Section II deals with range sensors- Hokuyo Laser Scanner and BumbleBee StereoVision Camera and the generation of their respective 2D cost maps. Section III deals sensor data fusion to generate an occupancy grid map and subsequent path planning. IV section is about the Lakshya s mechanical design and our results obtained from experiments performed on Lakshya. The paper is concluded in section V by discussing future works and applications in this field. 2. RANGE SENSORS A BumbleBee StereoVision camera by Point Grey Research, with two Sony 1/3 progressive scan CCDs and a resolution of 640x480 at 48FPS or 1024x768 at 18FPS, was also used in conjugation with the Hokuyo laser, on the Lakshya platform for stereo imaging of surroundings. Fig. 1 Image Sensing The LRF used in experimentation was Hokuyo s URG-04LX which has a range of 20mm to 4m. It has a 240 o scanning area with 0.36 o angular resolution. Laser beams strike off an object to determine its distance and direction. The scanning time is around 100msec/scan. Based on the position of objects around the robot a 2D map is generated. 22

2 compute the occupancy grid; whereas for regions II and III, both the laser and camera data is fused together. The drawback of the LRF is that it can detect only those objects which are lying in its line of scanning. Therefore objects like chairs and tables are represented by only their vertical supports and not as whole objects. Also objects lying below or above the scanning field of the laser are not detected and hence the robot can plan its path through them. This difficulty is overcome with the help of stereo vision camera, which uses 3D point cloud method to determine the position of obstacles in the environment. StereoVision Camera System A. 3D Point Cloud The stereo vision system constructs a 3-D view of the environment and then extracts the obstacles present in it. This is done by either generating disparity maps and evaluating them [6] [7], or generating 3D point cloud map of the environment using the disparity values. Another way to accomplish the above is to use v-disparity method [8] - a well known real time obstacle and ground detection algorithm, which was used in DARPA Grand Challenge and proposed by R. Labayrade et al. Using disparity values [9] and intrinsic and extrinsic camera parameters, each image pixel (x, y) is assigned a real world coordinate (X, Y, Z), where the Z-axis is in the direction of motion of the robot and the coordinate system is relative to the robot i.e. it moves with the robot. 3D coordinates are assigned [10] by finding out the corresponding points in left and right images and then using the calibrated camera parameters to determine its 3D position. Fig. 3 illustrates the 3D point cloud of the table formed by the stereovision camera. The 3D point cloud map thus generated for all the image pixels, is first pruned to reduce computational time. Fig. 2 Sensor Placements For initial experimentation the LRF was placed above the camera as shown in Fig.2. The 3D space in front of the robot is divided into 4 regions; region I and IV can be analyzed only by the laser and are out of the range of camera s vision; whereas region II and III are mapped by both the laser and the camera. For regions I and IV, only 2D laser map is used to Fig. 3 (clockwise from left) obstacle table; polar histogram generated by the laser of the obstacle; 3D point cloud of the obstacle by the stereovision camera. In the left image of Fig. 3, an obstacle, a table, is depicted. The 2D polar map generated by the laser (right image) is able to detect only the legs of the table, and marks the region between the table legs as traversable. On the other hand, the 23

3 3D point cloud generated by the stereovision camera is able to detect the whole table as an obstacle. B. Rationalizing the 3D Point Cloud To reduce computational burden and increase the efficiency of the algorithm, the 3D data so obtained has to be pruned. Pruning can be done in various steps- a) Ground Cancellation-Ground detection algorithms are applied to detect the ground, whose point cloud is subsequently deleted from 3-D point cloud as an effort to prune the data obtained from camera. One way to detect ground is to assume a constant horizontal ground plane underneath the robot. Another way is to use RANSAC (Random Sample Consensus) [11] technique, in which inliers constitute the ground plane whereas obstacles, ditches etc are rejected as outliers. C. Creation of 2D map from Rationalized 3D Point Cloud. A 2D vision map is created from the 3D point cloud generated from the camera data. The map cells are data structures with three integer variables and one float variable. int x = x-coordinate of cell; int y = y-coordinate of cell; int flag = 0(unoccupied) or 1(occupied) or Fig. 4 b) Deletion of points lying above height H - Obstacles lying above the height, H of robot do not hinder the motion of the robot and hence can be neglected from 3D point cloud (Fig. 4). The method has been described in [12]. c) Limiting the camera s range- The LRF has a range of 4 m. Though the range of stereo vision camera is infinite, but with increasing distance, noise in the data increases and accuracy decreases. Data beyond the range of R (here R is determined experimentally and depends on velocity, reaction time of the robot etc.) is eliminated and hence point clouds beyond this range, R are rejected which again minimalizes computational burden. d) Removing objects of height, h- Objects which are lying on the ground plane and are of a height, h, where, h << clearance of the robot are removed from the 3D point cloud as they are easily traversable. 2(unidentified); float r cell = range obtained from stereovision camera (0,0); The flag variable is assigned values 0 or 1 or 2, according to the following algorithm. Fig.5 Let the ground represent x-z plane; with the z-axis in the direction of motion of the robot. Let y-axis be the vertical axis. For a cell in the x-z plane, let its length and breadth be represented by Δz and Δx. In the pruned 3D point cloud, if n is the number of points in a cuboid of volume, V = Δx * Δz * y. then, flag = { σ > β max, 1 β min < σ < β max, flag value of cell lying in front of it σ < β min, 0 } where β is an experimentally determined threshold value. flag = 2, for cells which are out of range of camera. Thus each cell of the matrix stores a 0 or 1 or 2 value corresponding to the placement of the obstacles. Fig.5 is a line representation of how an obstacle is projected onto a 2D map to determine occupancy values of the cells of vision map. 24

4 III SENSOR FUSION TO GENERATE OCCUPANCY GRID AND PATH PLANNING The 2D matrix obtained after pruning and scaling down 3D point cloud is fused with the 2D map generated by the laser to create an occupancy grid. A) Assigning flag value to Occupancy grid The occupancy grid can contain four different values or weights- unoccupied (0), occupied (1), unidentified (2) and unsure (3). The cells are assigned the above values on the basis of the following parameters- The occupancy grid so generated is updated with each laser and camera scan, which enables the robot to move in a dynamic environment. The 2D occupancy grid created after the fusion of both the sensors data, is used for obstacle avoidance and path planning. The algorithm used for this purpose is the improved version of Vector Field Histogram (VFH) i.e. VFH+ [13]. The algorithm reduces the 2D occupancy grid to a 1D polar histogram and labels free spaces as candidate valleys, from which the direction of the motion for the robot is determined. Advantages of VFH+ are that besides path planning, it also takes into account the dynamics and kinematics of the robot while planning an optimum path through the obstacles. 1) A cell which is marked as occupied in both the laser 2D map and the stereovision camera s 2D map or only in the laser 2D map- is assigned a value of 1(occupied). 2) A cell identified as empty by both the 2D maps is given a value of 0 (unoccupied). 3) Cells lying beyond cells identified as definite obstacles are marked as unidentified (2). 4) Cells identified as obstacle by camera and detected as free space by the laser are assigned 1(occupied)(?). B) Assigning Range Value to Occupancy grid Range values are assigned in the occupancy according to the pseudo-algorithm (fig. 6) given below. Pseudo algorithm for a cell of the occupancy grid, If ( r laser - r camera < Δr ) r cell = r laser ; else if ( r laser doesn t exist ) r cell = r camera ; else if ( r camera doesn t exist ) r cell = r laser ; else r cel l is unidentified; Fig. 6 where, r cell is the final of range for the cell to be used by obstacle avoidance algorithm; Δr is an experimentally determined value; r laser and r camera are the range values given for an obstacle by the laser and the camera respectively. r laser doesn t exist in regions beyond 4m. For example, if the laser detects an object at 1m and the camera detects the obstacle at 1.01m, then the difference =0.01m can be considered as negligible and the corresponding cells(which show the object at 1.0m) of the occupancy grid are marked as occupied(1). IV HARDWARE SETUP AND EXPERIMENTAL RESULTS The chassis of the robot is made of welded aluminum bars and sheet metal and the wheels were powered by 2 Quicksilver Servo Motors. Hokuyo s URG-04LX and BumbleBee StereoVision camera are the sensors installed for obstacle detection and avoidance. Obstacle avoidance program is written in Microsoft Visual Studio 9.0 in C++ environment. It uses PGR library and Intel OpenCV Library for Navigation Map building. The proposed techniques and algorithms were tested successfully on Lakshya - an IGV platform conceptualized and developed in the Innovations Lab, Delhi College of 25

5 Engineering, in both indoor and outdoor environments. The robot was able to successfully identify and avoid stationary and dynamic objects of various shapes and configurations. In fig. 7, for experimentation the laser range has been limited to 2.5 m, hence the obstacle(box) is detected only by the camera and not the laser. In fig. 9, the camera is able to detect only obstacle. In fig. 7 and 8, only the LRF is able to detect the helmet, kept at the right of the robot, as an obstacle as it lies out of the FOV of the camera. In fig. 10 the robot has taken a turn to avoid the obstacle hence the obstacle moves out of the FOV of camera and is detected only by the laser scan. Fig. 7 (clockwise from top left) obstacle; 2D map by laser(when the range of the laser is set to 2.5 m); 2D map by stereovision camera; sensor fusion-occupancy grid. Fig. 9 (clockwise from top-left) obstacle; 2D by laser; 2D navigation map by stereovision camera; sensor fusionoccupancy grid. Fig. 8 (clockwise from top-left) obstacle; laser 2D cost map; 2D camera vision map; fused map. Fig. 10 (clockwise from top-left) obstacle detection; 2D map by laser; 2D map by stereovision camera - obstacle lies outside its view; sensor fusion-occupancy grid. part of the obstacle as it lies partly out of the field of view of the camera; whereas the laser is able to detect the whole of the 7. CONCLUSION AND FUTURE WORK 26

6 In this paper we have presented a method of sensor fusion of LRF and stereovision camera to generate 2D occupancy grids for obstacle detection and avoidance. As has been illustrated by the experimental results, both laser and camera data are necessary for successful path navigation. The accuracy of the laser is used to complement the 3D imaging properties of the stereovision camera. Our paper mainly focuses on reducing the computational burden involved in fusion process and accurate detection and avoidance of obstacles in the environment of a mobile robot. Future Work can brought together by focusing on data fusion technologies which are based on Bayesian inference and probabilistic reasoning as they are relatively better off in case of uncertainties available in the environment. REFERENCES [1] S. Thrun, D. Fox, and W. Burgard. A real-time algorithm for mobile robot mapping with application to multi robot and 3D mapping, in proceedings of the IEEE Int. Conf. on Robotics and Automation (ICRA 00), USA, [2] Haris Baltzakis, Antonis Argyros, Panos Trahanias, Fusion of laser and visual data for robot motion planning and collision avoidance, Machine Vision and Applications (2003) 15: V disparity representation, in IEEE Intelligent Vehicle Symposium, Versailles, June [9] Alper Yilmaz, Sensor Fusion in Computer Vision, IEEE Urban Remote Sensing Joint Event, 2007, April 2007 Page(s):1-5 [10] S.Nendevschi, R.Dancescu, D. Frentiu, T.Martia, F.Oniga, C.Pocol, R.Schmidt, T.Garf, High Accuracy Stereo Vision System for Far Distance Obstacle Detection, IEEE Intelligent Vehicle Symposium, Parma, Italy, 2004 [11] M.A. Fischler and R.C. Bolles, Random consensus: A paradigm for model fitting with applications to image analysis and automated cartography. Communications of the ACM, 1981, 24(6), [12] Saurav Kumar, Binocular Stereo Vision Based Obstacle Avoidance Algorithm for Autonomous Mobile Robots, IEEE Advance Computing Conference 2009 [13] Iwan Ulrich and Johann Borenstein, VFH+: Reliable Obstacle Avoidance for Fast Mobile Robots, in 1998 IEEE International Conference on Robotics and Automation. [3] Mathias Perrollaz, Raphael Labayrade, Cyril Royere, Nicolas Hautiere, Didier Aubert, Long Range Obstacle Detection Using Laser Scanner and Stereovision, in Intelligent Vehicles Symposium 2006, June 13-15, 2006, Tokyo, Japan. [4] C. Stiller, J. Hipp, C. Rossig, A. Ewald, Multisensor obstacle detection and tracking, Image and Vision Computing, Volume 18, Issue 5, April 2000, Pages [5] Romuald Aufrere, Jay Gowdy, Christoph Mertz, Chuck Thorpe, Chieh-Chih Wang, Teruko Yata, Perception for collision avoidance and autonomous driving, Mechatronics, Volume 13, Issue 10, December 2003, Pages [6] Zhuoyun Zhang, Chunping Hou, Lili Shen, Jiachen Yang, An Objective Evaluation for Disparity Map based on the Disparity Gradient and Disparity Acceleration, 2009 International Conference on Information Technology and Computer Science. [7] K. L. Boyer, D. M. Wuescher, and S. Sarkar, Dynamic Edge Warping: An Experimental System for Recovering Disparity Maps in Weakly Constrained Systems, IEEE TRANSACTIONS ON SYSTEMS, MAN, AND CYBERNETICS, VOL. 21, NO. 1 JANUARY/FEBRUARY 1991 [8] R. Labayrade, D. Aubert, and J.P. Tarel, Real time obstacle detection on non flat road geometry trough 27