Building a grid-point cloud-semantic map based on graph for the navigation of intelligent wheelchair

Transcription

1 Proceedings of the 21th International Conference on Automation & Computing, University of Strathclyde, Glasgow, UK, September 2015 Building a grid-point cloud-semantic map based on graph for the navigation of intelligent wheelchair Cheng ZHAO*, Huosheng HU, Dongbing GU School of Computer Science and Electrical Engineering, University of Essex CO4 3SQ, Colchester, UK IRobotCheng@gmail.com, hhu@essex.ac.uk, dgu@essex.ac.uk Abstract The automatic navigation of the intelligent wheelchair is a major challenge for its applications. Most previous researches mainly focus on 2D navigation of intelligent wheelchair, which loses many useful environment information. This paper proposed a novel Grid-Point Cloud-Semantic Multi-layered Map based on graph optimization for intelligent wheelchair navigation. For mapping, the 2D grid map is at the bottom, the 3D point cloud map is on the grid map and the semantic markers are labelled in them. The semantic markers combine the name and coordinate value of object marker together. For navigation, the wheelchair uses the grid map for localization and path planning, uses the point cloud map for feature extraction and obstacle avoidance, and uses the semantic markers for humanrobot vocal interaction. A number of experiments are carried out in real environments to verify its feasibility. Keywords Graph-based SLAM, Grid Map, Point Cloud Map, Semantic Labelling, Navigation I. INTRODUCTION Intelligent wheelchair is a very useful assisted living application for the elderly and disabled people, which can decrease the pressure of ageing of population. The availability of automatical navigation of intelligent wheelchair is necessary in hospital, office and home. In the past decade, there are many notable successes for the simultaneous localization and mapping(slam) in robotics community. There are four mainly kinds of typical map now: grid map, point cloud map, topological map and semantic map. There are many navigation researches that focus on different type of maps. The outline of grid and point cloud mapping is similar and it can be classified as filtering, smoothing and graphbased approaches. The state of filtering approach consists in current robot position and map, which can perform a online state estimation. When the new measurement is available, the estimate is augmented. The Kalman [1][2], particle [3][4], information [5][6] filter are very popular online SLAM methods. In term of smoothing approach [7][8] that relies on leastsquare error minimization estimate the whole trajectory of robot through the whole set of measurements. Recently, the graph-based SLAM [9][10][11] achieved many breakthroughs, which constructs a graph out of the raw sensor measurements. The graph consists of the nodes each of which represents the robot position with the measurements from this position and edges each of which represents the spatial constraint between two robot poses. The constraint is actually the transformation relating two robot poses and it can be calculated through odometry measurements or aligning the observations. After constructing the graph, the next step is graph optimization: calculating a most likely configuration that best satisfies the constrains. However online processing for global loop closure detection is difficult in large-scale and long-term environment, the work in [12] proposed a graph-based memory management approach that only considers portions of map satisfy online processing requirements. The only difference between grip and point cloud mapping is that replacing the 2D scan matching and loop closure detecting with counterparts that operate 3D point cloud. The work in [13] proposed a hybrid map that integrates grid and topological map for indoor navigation. The work in [14] proposed an approach to segment and detect the room spaces for navigation using range data in office environment. Using the anchoring technique, [15] labels the topological map with semantic information for navigation. The works in [16][17] introduced an approach to learn topological maps from geometric map by applying semantic classification procedure based on associative Markov networks and AdaBoost. Many work used visual features from camera sensor like [18] to extract semantic information via place categorization. The paper [19] integrated a robust image labelling method with a dense 3D semantic map. In addition, many work added rich semantic information to the map through human-robot interaction for navigation. In [20], a contextual topological map was built through making the robot follow the user and verbal commentary. The work in [21] built the semantic map through clarification natural language dialogues between human and robot. The system in [22] integrated laser and vision sensors for place and object recognition as well as a linguistic framework, which create conceptual representation of human-made indoor environment. The work in [23] summarized many multi-modal interactions such as speech, gesture and vision for semantic labelling, which can make the robot get rich environment knowledge without many pre-requisites features. Different types of map have their own advantages and disadvantages for navigation. Grid map is easy to make path planning but it does not have enough 3D features and semantic information. Point cloud map is easy to get enough features information but it is hard to use for path planning because of high complexity. Semantic map can provide the knowledge of environment for human-robot interaction but the robot can not perform any action based on it. It is necessary to combine different type of map together in order to make the navigation of robot more efficiently. This paper proposes a grid-point cloudsemantic multi-layered map for the navigation of intelligent wheelchair and carries out some related experiments in real environment. The rest of this paper is organized as follows. Section 2 introduces the intelligent wheelchair platform and outlines the basic system workflow. Section 3 outlines the 222

2 approach deployed in this research. In section 4, some related experiment results are presented to verify the performance of the proposed approach. Finally, section 5 makes a brief conclusion and future work. A. Robot Platform II. SYSTEM OVERVIEW The platform used in this work is a commercial intelligent wheelchair Spectra Plus equipped with a Kinect which provides RGB and depth information, a Hokuyo URG-04LX- UG01 Laser which provides 2D laser scan information and optical encoders which provide odometry transformation information. Computation is preformed on an on board PC (Intel Pentium(R) 2.30 GHz, 8G RAM and Integrated Graphics) installed with Ubuntu and ROS Indigo. The platform snapshot is shown in Figure 1. B. Architecture Overview Fig. 1: The platform snapshot. The mapping module can be broken up into three modules: Frontend, Backend and Map creation. The frontend mainly obtains the sensor data, extracts the features and calculates the geometric relationships between robot and landmarks. The backend mainly constructs a graph, preforms loop closure detection and optimizes the graph structure. Finally, this module outputs a maximum likelihood solution of the robot trajectory. In addition, it also performs a model matching through comparing the object database with the features from RGB frames. If the model matching is successful, the mass centre position of the object will be calculated through the depth images. This position is combined with the object name together in a semantic marker. In terms of map creation, the 2D grid map and 3D point map are generated based on the trajectory. Then the semantic markers are labelled to them. Combining them together, the multi-layered map is finally generated. The navigation module consists of localization, getting a destination, path planning. The wheelchair can perform the localization based on the trajectory from graph SLAM. Then the user tells the wheelchair the destination name using vocal interface. The wheelchair can get the position of destination from semantic markers. Finally, the wheelchair performs path planning using grid map and obstacle avoidance using point cloud map. The architecture overview is shown in Figure 2. A. Pose graph outline III. APPROACHES The map of graph-based is a graph consisting of nodes and edges like G = {V,E}. The nodes [12] contain the odometry poses, laser scans, RGB and depth images of each location. In addition, they also save visual words which are used for loop closure detection. The edges are mode of neighbors and loop closures, which save the geometrical transformation between nodes. When the odometry transformation between the current and previous nodes is generated, the neighbor edges are added to the graph. When a loop closure detection is found between the current node and one of the previous maps, the loop closure edges are added to the graph. After the construction of graph, the graph optimization is performed which calculates the most likely configuration that best satisfies the constraints of edges. Suppose x =(x 1...x 2 ) T is a vector of parameters which represent the configuration of nodes. ω ij and Ω ij represent the mean and the information matrix of the observation of node j and i respectively. For the state x, f ij (x) is the function which can calculate the observation of the current state. The residual r ij can be calculated via equation (1). r ij (x) =ω ij f ij (x) (1) The amount of error introduced by constraints which are calculated by real or visual odometry, weighed by its information, can be obtained via equation (2). d ij (x) 2 = r ij (x) T Ω ij r ij (x) (2) Assuming all the constraints to be independent, the overall error can be calculated through equation (3). D 2 (x) = d ij (x) 2 = r ij (x) T Ω ij r ij (x) (3) where d ij (x) 2 is residual of the edge which connects node i and j in graph φ. So the key of graph-based SLAM is to find a state x that minimizes the overall error. x =argmin x r ij (x) T Ω ij r ij (x) (4) Compactly, it is rewritten in equation (5) x =argmin r ij (x) 2 ij x where ij =Ω 1 ij is the corresponding covariance matrix of the information matrix. B. Loop closure detection The approach of loop closure detection [12][24] is used in this work. They use a Bayesian filter to evaluate loop closure hypotheses over all previous images. If the hypothesis reaches the pre-defined threshold H, a loop closure is detected. In order to compute the likelihood required by the filter, the ORB features is used for an incremental visual words considering the low computing power of the on board PC. If you have a powerful on board PC, the best choice is SIFT feature with GPU. For every 2D point from RGB image where the visual words are extracted, the relating 3D position can be calculated via calibration matrix and the depth image. So, every visual (5) 223

3 Vocal interface RGB Features extraction RANSAC Model matching Position calcultation Semantic markers Destination position Kinect Depth Point cloud subsampling ICP Object features database 2D map creation Path planning Wheel odometry Transformation Loop detection& Graph optimization Trajectory Multi-layered map Reach the destination Laser 2D Scan Scan Matching 3D map creation Obstacle avoidance Frontend Backend Map creation Navigation Fig. 2: Architecture overview. words 3D position can be obtained. Using the 3D visual word correspondences, every transformation between the matching images is computed via RANSAC if a loop closure is found. When the minimum of I inliers is found, the loop closure is successful and the related edge is added to the graph. C. Graph optimization For graph optimization, the G2O [25][26] framework is used in our work. This approach performs a minimization of non-linear error function that can be represented as a graph. G2O can minimize the error in a graph in which vertices are parameterized by transformation and edges represent constraints between vertices with associated covariance matrices. It maximizes the likelihood of the vertex parameters subject to the constraints using stochastic gradient descent. When the robot revisits a known part of the map, the loop closure edges in the graph can diminish the accumulated error introduced by odometry. D. Map representation Based on the trajectory outputted from graph-based SLAM, the laser scans are assembled together and the 2D grid map is generated at the bottom, meanwhile the RGB and depth images are assembled together and the 3D point cloud map is generated on the grid map. In terms of semantic labelling, firstly a database of objects features is built. Then a model matching is performed comparing the extracted features from RGB images and the features in object database. If a model matching is successful, the centre of mass position of the found object can be calculated through corresponding depth image. This position is combined with object name in a semantic marker. Due to the low computing power of the on board PC, the ORB feature is selected as the feature extraction and description. Lastly, the semantic markers are labelled in the 2D and 3D map. E. Navigation Where am I, Where shall I go and How to get there are the three central issues of navigation. For the first issue, the intelligent wheelchair can get its position through the trajectory outputted from graph-base SLAM. For the second issue, the user can tell wheelchair the destination name using vocal interface through bluetooth earphone. The open source speech recognition toolkit Pocket Sphinx [27] is used for humanrobot vocal interaction, which is easy to use for training(just upload the semantic text to the tool website). The voice will be transformed to text and then a string matching is performed. Finally the wheelchair can find the position of corresponding destination name from semantic markers. For the third issue, the wheelchair transforms the grid map to a cost map [28][29], and then uses Dijkstra algorithm to search the cost map to find a route that has minimal sum of the cost value. For obstacle avoidance, the obstacles and ground are segmented by normal filtering using point cloud map. All points with normal in the z direction are labelled as ground and all the others are labelled as obstacles. Lastly, movebase package [30] in ROS is used for the motion planning of intelligent wheelchair. A. 2D grid map IV. EXPERIMENTS As shown in the Figure 3, the 2D grid map is generated using laser scans. The blue line represents the trajectory of the wheelchair and the small blue point represents the graph node. The yellow line connected with two nodes represents that there is a loop closure between them. There are many loop closures in A, B, C because the wheelchair rotated by 360 degrees there, while there are also many loop closures in D, E, F because the wheelchair moved with a loop trajectory. B. 3D point cloud map As shown in the Figure 5, the 3D point cloud map is generated using RGB-D information. The whole point cloud map are made of a large number of RGB-D frames obtained from every node in the graph. Those frames are aligned together through the transformation constrains. C. Semantic labelling The object database stores the ORB features of Baxter, Fox robot, Robot fish, Fly robot and Robot dog. By extracting the 224

4 F D A B D E F C B A E C Fig. 3: 2D grid map. Fig. 4: 3D semantic labelling. 225

5 Fig. 5: 3D point cloud map. B C G A H F E D Fig. 6: Multi-layered map. 226

6 Fig. 7: Navigation in hybrid map. ORB features from RGB images, these robots were found in the process of mapping, as show in the colorized box from Figure 4. The positions of their centre mass were calculated using the depth image. As shown in Figure 4, there is a yellow line connecting the wheelchair and the object marker. Finally, those semantic markers (X, Y, Z) will be labelled in the 3D point cloud map. D. Multi-layered map As shown in the Figure 6, the multi-layered map is generated: the grid map A is at the bottom, the point cloud map B is on the grid map and the semantic markers C, D, E, F, G are labelled as green points in the point cloud map. The green line represents the trajectory of the wheelchair. E. Navigation As shown in the Figure 7, in the point cloud map, the obstacles are inflated and labelled as red colour, the ground is labelled as green colour. The blue and red line on the grid map are the outcomes of global and local path planning respectively. All the mapping and navigation videos of experiments can be found Tl5R4MBKIXsGI0sFlJuXQ V. CONCLUSION As there are some obviously drawbacks when the intelligent wheelchair performs navigation only using one single type of map, in this paper a Grid-Point Cloud-Semantic Multilayered map based on graph optimization for the navigation is proposed. The grid map at the bottom is used for localization and path planning, the point cloud map on the grid map is used for features extraction and obstacles avoidance, the semantic markers is used for human-robot vocal interaction. This hybrid map can make intelligent wheelchair perform navigation task more efficiently. In the future, our work will mainly focus on adding more rich semantic information to this hybrid map through 3D labelling. ACKNOWLEDGEMENT The 1st author is financially supported by scholarships from China Scholarship Council and University of Essex, U.K. REFERENCES [1] C. P. Smith R, Self M, Estimating Uncertain Spatial Relationships in Robotics. Springer New York, [2] M. W. M. Gamini Dissanayake, P. Newman, S. Clark, H. F. Durrant- Whyte, and M. Csorba, A solution to the simultaneous localization and map building (SLAM) problem, IEEE Transactions on Robotics and Automation, vol. 17, no. 3, pp , [3] M. Montemerlo, S. Thrun, D. Koller, and B. Wegbreit, FastSLAM: A factored solution to the simultaneous localization and mapping problem, in Proc. of 8th National Conference on Artificial Intelligence/14th Conference on Innovative Applications of Artificial Intelligence, vol. 68, no. 2, 2002, pp [4] G. Grisetti, C. Stachniss, and W. Burgard, Improved techniques for grid mapping with Rao-Blackwellized particle filters, IEEE Transactions on Robotics, vol. 23, pp ,

7 [5] R. M. Eustice, H. Singh, and J. J. Leonard, Exactly sparse delayed-state filters, in Robotics and Automation, ICRA Proceedings of the 2005 IEEE International Conference on. IEEE, 2005, pp [6] S. Thrun, Y. Liu, D. Koller, A. Ng, Z. Ghahramani, and H. Durrant- Whyte, Simultaneous Localization and Mapping with Sparse Extended Information Filters, vol. 23, no. 7, pp , [7] F. Dellaert and M. Kaess, Square Root SAM: Simultaneous Localization and Mapping via Square Root Information Smoothing, The International Journal of Robotics Research, vol. 25, no. 12, pp , [8] E. Olson, J. Leonard, and S. Teller, Fast iterative alignment of pose graphs with poor initial estimates, in Proceedings - IEEE International Conference on Robotics and Automation, vol. 2006, no. May, 2006, pp [9] S. Thrun, The Graph SLAM Algorithm with Applications to Large- Scale Mapping of Urban Structures, The International Journal of Robotics Research, vol. 25, no. 5-6, pp , [10] H. Strasdat, J. M. M. Montiel, and A. J. Davison, Visual SLAM: Why filter? Image and Vision Computing, vol. 30, no. 2, pp , [11] F. Endres, J. Hess, J. Sturm, D. Cremers, and W. Burgard, 3-D Mapping with an RGB-D camera, IEEE Transactions on Robotics, vol. 30, no. 1, pp , [12] M. Labbe and F. Michaud, Online global loop closure detection for large-scale multi-session graph-based SLAM, in 2014 IEEE/RSJ International Conference on Intelligent Robots and Systems, 2014, pp [13] S. Thrun and A. Bücken, Integrating Grid-Based and Topological Maps for Mobile Robot Navigation, Proceedings Of The National Conference On Artificial Intelligence, vol. 13, no. August, pp , [14] P. Buschka and A. Saffiotti, A virtual sensor for room detection, IEEE/RSJ International Conference on Intelligent Robots and Systems, vol. 1, no. October, pp , [15] C. Galindo, A. Saffiotti, S. Coradeschi, P. Buschka, J. a. Fernández- Madrigal, and J. González, Multi-hierarchical semantic maps for mobile robotics, in 2005 IEEE/RSJ International Conference on Intelligent Robots and Systems, IROS, 2005, pp [16] O. Mozos and W. Burgard, Supervised Learning of Topological Maps using Semantic Information Extracted from Range Data, 2006 IEEE/RSJ International Conference on Intelligent Robots and Systems, pp , [17] E. Brunskill, T. Kollar, and N. Roy, Topological mapping using spectral clustering and classification, 2007 IEEE/RSJ International Conference on Intelligent Robots and Systems, pp , [18] J. Wu, H. I. Christensen, and J. M. Rehg, Visual place categorization: Problem, dataset, and algorithm, in 2009 IEEE/RSJ International Conference on Intelligent Robots and Systems, IROS 2009, 2009, pp [19] Z. Zhao and X. Chen, Semantic mapping for object category and structural class, in 2014 IEEE/RSJ International Conference on Intelligent Robots and Systems, no. Iros, 2014, pp [20] A. Diosi, G. Taylor, and L. Kleeman, Interactive SLAM using laser and advanced sonar, in Proceedings - IEEE International Conference on Robotics and Automation, vol. 2005, no. April, 2005, pp [21] G.-J. M. Kruijff, H. Zender, P. Jensfelt, and H. I. Christensen, Clarification dialogues in human-augmented mapping, Proceeding of the 1st ACM SIGCHI/SIGART conference on Human-robot interaction - HRI 06, p. 282, [22] H. Zender, O. Martínez Mozos, P. Jensfelt, G. J. M. Kruijff, and W. Burgard, Conceptual spatial representations for indoor mobile robots, Robotics and Autonomous Systems, vol. 56, no. 6, pp , [23] G. Randelli, T. M. Bonanni, L. Iocchi, and D. Nardi, Knowledge acquisition through human-robot multimodal interaction, Intelligent Service Robotics, vol. 6, no. 1, pp , [24] M. Labbe and F. Michaud, Appearance-based loop closure detection for online large-scale and long-term operation, IEEE Transactions on Robotics, vol. 29, pp , [25] R. Kümmerle, G. Grisetti, H. Strasdat, K. Konolige, and W. Burgard, G2o: A general framework for graph optimization, in Proceedings - IEEE International Conference on Robotics and Automation, 2011, pp [26] G. Grisetti, S. Grzonka, C. Stachniss, P. Pfaff, and W. Burgard, Efficient estimation of accurate maximum likelihood maps in 3D, in IEEE International Conference on Intelligent Robots and Systems, no. Ml, 2007, pp [27] D. Huggins-Daines, M. Kumar, A. Chan, A. Black, M. Ravishankar, and A. Rudnicky, Pocketsphinx: A Free, Real-Time Continuous Speech Recognition System for Hand-Held Devices, 2006 IEEE International Conference on Acoustics Speech and Signal Processing Proceedings, vol. 1, pp , [28] J. King and M. Likhachev, Efficient cost computation in cost map planning for non-circular robots, in Intelligent Robots and Systems. St.Louis,MO: Ieee, Oct. 2009, pp [29] E. Marder-Eppstein, E. Berger, T. Foote, B. Gerkey, and K. Konolige, The Office Marathon: Robust navigation in an indoor office environment, in IEEE International Conference on Robotics and Automation. Anchorage,AK: Ieee, May 2010, pp [30] B. P. Gerkey and K. Konolige, Planning and Control in Unstructured Terrain, in ICRA Workshop on Path Planning on Costmaps,