Robotic exploration for mapping and change detection

Transcription

1 Masterarbeit Sebastian Gangl Robotic exploration for mapping and change detection Robotische Exploration zur Kartierung und Änderungsdetektion Erstprüfer: apl. Prof. Dr.-Ing. Claus Brenner Zweitprüfer: Dr.-Ing. Jens-André Paffenholz

2

3 Robotic exploration for mapping and change detection Master thesis at ikg and GIH Introduction and scope of work Exploration is the process of controlling a robot in such a way that it maximizes its knowledge about its state. When a map is given, this state consists of the robot s pose, and the task is also known as active localization. If the map is unknown, exploration seeks to find the optimal control to localize the robot and build the map. Environments are usually not static. While moving objects pose no problem to standard SLAM algorithms, slowly changing scene parts may yield false localization results. Therefore, it is our goal to use robotic exploration to identify changes in the scene. In this thesis, the task will be to review, select, implement and evaluate algorithms for the robotic exploration of an environment, as well as the detection of changes. The ikg floor will serve as a test bed. The basic scenario is to start the robot at an arbitrary (unknown) position, from where it will map the entire floor, using active exploration. It will then align and compare this map with a previously generated map and identify the changes that have occurred between the two mapping campaigns. The changes will be quantified with respect to geometric extent (size) and possibly also with regard to their change rate (using multiple campaigns). Tasks Literature review: active localization, exploration and change detection. Implementation of active localization using a given map (e.g. a floor plan). It is expected, although not required, that algorithms will be based on an occupancy grid representation. Realisation of exploration in an unknown environment. Different strategies may also be evaluated by means of simulations. Use of the previous results to perform change detection for an unknown environment. Analysis and assessment of the implemented algorithms. Documentation of the results. Resources Volksbot robot (presumably RT3 w/ Laser scanner SICK LMS100). ROS packages (e.g. navigation, move_base, hector_exploration_planner). Indoor test environment (floor plan of ikg is available). Requirements Programming skills: C++, Python, Robot Operating System (ROS). Contact persons ikg: apl. Prof. Dr.-Ing. Claus Brenner ( Claus.Brenner@ikg.uni-hannover.de) Dipl.-Inf. Colin Kuntzsch ( Colin.Kuntzsch@ikg.uni-hannover.de) GIH: Dr.-Ing. Jens-André Paffenholz ( paffenholz@gih.uni-hannover.de) German title: Robotische Exploration zur Kartierung und Änderungsdetektion.

4

5 Statutory Declaration I assure that this thesis is a result of my personal work and that no other than the indicated aids have been used for its completion. Furthermore I assure that all quotations and statements that have been inferred literally or in a general manner from published or unpublished writings are marked as such. Beyond this I assure that the work has not been used, neither completely nor in parts, to pass any previous examination. Hannover, Sebastian Gangl Eidesstattliche Erklärung Ich versichere hiermit, dass ich die vorliegende Masterarbeit selbstständig und ohne Benutzung anderer als der angegebenen Quellen und Hilfsmittel angefertigt habe. Alle Zeichnungen und Abbildungen, die mit keinem Quellennachweis versehen sind, wurden von mir selbst erstellt. Hannover, Sebastian Gangl

6

7 Abstract The scope of this thesis is to solve the active localization problem, as well as the active exploration problem. Last but not least, the change detection should be realized, too. To solve the active localization problem, a greedy information based algorithm is used. For the active exploration, three different strategies are discussed and compared by their advantages and disadvantages. The frontier-based exploration strategy is implemented by means of simulation and of a real robot. Last, to realize the change detection, 2D multi-class occupancy grid maps and the Kullback-Leibler divergence are used. To implement the discussed algorithms for exploration and change detection, the Robot Operating System is used and to realize the simulation, Gazebo is used. Using the greedy information based strategy to solve the active localization, the algorithm is able to solve the problem, if the state consists of two particle clusters. But for a real application, the computational complexity is a huge drawback and therefore, at the current stage no greedy information based strategy for active localization can be recommended. The frontier-based exploration is an efficient strategy to explore an unknown environment. The algorithm works by means of simulation and of a real robot. Last but not least, the change detection works with the 2D multi-class occupancy grid maps and the Kullback-Leibler divergence. But the implementation of this approach is not very robust. The central questions in robotics are localization, perception and model building. This thesis contributes to all three of them.

8

9 Contents Contents List of Figures List of Tables i iv vii 1 Introduction Motivation Scope of Thesis Structure of Thesis Theory and Background Odometry-based Motion Model Probabilistic Filter - Bayes Filter Kalman Filter Particle Filter Representation of the Environment - Occupancy Grid Map Adaptive Monte Carlo Localization Motion Model for the Prediction Step Measurement Models for the Update Step Rao-Blackwellized Particle Filter Occupancy Grid SLAM Path Planning Non-optimal Path Planning Optimal Path Planning Additional Methods for Path Planning Exploration Known Environment - Active Localization Unknown Environment - Active Exploration Change Detection Sensors and Software Volksbot RT Sensors Odometry Laser Scanner Sick LMS Coordinate Systems Coordinate System of the Robot Coordinate Systems of the Sensors i

10 Contents ii 3.4 Software ROS - Robot Operating System Gazebo Scenarios, Implementation and Experiments Map of Basic Scenario - IKG Floor ROS-Driver for Odometry Autonomous Navigation with Collision Avoidance Exploration Active Localization Active Exploration Change Detection Experiments Active Localization Active Exploration Change Detection Results and Evaluation Challenges with AMCL and move base Active Localization Evaluation of the Active Localization by Simulated Data Best Parameter Combination Active Localization by Means of a Real Robot Active Exploration Simulation with Gazebo Evaluation of Exploration Strategy - Frontier Detection Active Exploration by Means of a Real Robot move base in Conjunction with GMapping Comparison of Different Active Exploration Strategies Change Detection Conclusion and Outlook Conclusion Outlook A Description and Configuration of AMCL Parameters 86 A.1 Description of the Parameters of the AMCL A.2 Determination of Parameters A.3 Configuration of AMCL B Description and Configuration of move base 94 B.1 Description of the move base B.2 Configuration of move base B.2.1 base local planner B.2.2 Costmap Parameters

11 Contents iii C ROS 102 C.1 Own Created ROS Nodes C.2 Communication of ROS Nodes C.3 Start the different Tasks C.4 Add Model to Gazebo D Supplementary Figures 110 D.1 Results of the Active Localization D.2 Results of the Exploration by Means of Simulation D.3 Results of the Exploration by Means of a Real Robot E Content of DVD 115 Bibliography 116

12 List of Figures 1.1 Examples for mobile robots and full or partial autonomous systems in our daily life Examples for mobile robots to be used in complex environments Examples for vehicles of the DARPA Grand/Urban Challenge in 2005 and Illustration of the motion: robot moves forward and turns left Illustration of the Markov assumption as a dynamic Bayesian network Illustration how the resampling wheel selects the particles Illustration how the low variance sampler selects the particles Example of an occupancy grid map Illustration of the translational and rotatory parts of a robots motion between two poses x t 1 and x t using an odometry-based model Possible density of the mixture distribution for the beam model Example of a pre-computed likelihood field of an occupancy grid map Illustration of frontier cells and frontier regions Example for polygonal lines, free curves and safe region for the viewpointbased exploration Overview of possible methods to implement algorithms for change detection Example of a 2D multi-class occupancy grid map with three classes Different views of the Volksbot RT3 from Frauenhofer IAIS Increase of uncertainties of position and heading by time Bi-directional square path experiment Visualization of a measurement from a laser scanner Coordinate system of the robot Coordinate system of the odometry Coordinate system of the laser scanner Example for visualization with the tool RVIZ Possible output of the tool rqt graph Examples of 3D models using Gazebo Line map of the IKG floor used as occupancy grid map in ROS The footprint of the robot with its coordinates Visualization of perception of the chassis with/without filtering the laser scanner data The transformation tree for using AMCL node Examples for valid and invalid poses in context of an occupancy grid map D model of the basic scenario, created with Sketchup iv

13 List of Figures v 4.7 3D model of the base of the Volksbot RT Detailed 3D model of the Sick LMS The transformation tree for using GMapping node Illustration of the case that new targets available after loop closing Visualization of the initial situation of the first test scenario for the active localization Visualization of the initial situation of the second test scenario for the active localization Visualization of the initial situation of the third test scenario for the active localization Visualization of the initial situation of the third test scenario for the active localization Illustration of the different start locations for evaluating the active exploration Initial situation for change detection. The robot maps an empty room Second configuration of the room. Now, an obstacle is added to the room Third configuration of the room. Changed the position of the object Created map with GMapping, to overcome the problems, which arose by using a gray value picture of the blue print Result of the greedy search for the next best exploration action for the first test scenario Where the robot would move to, if the chosen exploration action a exp = (5.0m, 5.0m) would be executed Result of the greedy search for the next best exploration action for the fourth scenario Where the robot would move to, if the chosen exploration action a exp = ( 7.0m, 0.0m) would be executed Result of the active localization for different α s D model of the robot and its sensors Combination of the 3D models from the robot and the basic scenario, using Gazebo Result of the exploration, starting the exploration by means of simulation the with 3 labeled start position Detailed illustration of the exploration for start position Result of the exploration, setting the Volksbot RT3 at the with 1 labeled start position Result of the exploration, setting the Volksbot RT3 at the with 3 labeled start position Result of mapping process for change detection A.1 Result of the update step for the beam model A.2 Result of the update step for the likelihood field model A.3 Result of the bi-directional square path experiment A.4 Start pose and driven trajectory for determination of the likelihood field model parameters for the AMCL A.5 Start locations to check which are maximal required particles to solve the global localization problem

14 List of Figures vi B.1 Summary of the move base and how the different subtasks of the move base interact with each other B.2 Summary of the executed behaviors to unstuck the robot C.1 Communication the ROS nodes for active localization C.2 Communication of the ROS nodes for simulation and autonomous exploration C.3 Communication the ROS nodes for autonomous exploration executing on the Volksbot RT D.1 Result of the greedy search for the next best exploration action to execute to determine the pose of the robot for the second scenario D.2 Result of the greedy search for the next best exploration action to execute to determine the pose of the robot for the third scenario D.3 Result of the exploration by means of simulation, setting the Volksbot RT3 at the with 1 labeled start position D.4 Result of the exploration by means of simulation, setting the Volksbot RT3 at the with 2 labeled start position D.5 Result of the exploration by means of simulation, setting the Volksbot RT3 at the with 4 labeled start position D.6 Result of the exploration, setting the Volksbot RT3 at the with 2 labeled start position D.7 Result of the exploration, setting the Volksbot RT3 at the with 4 labeled start position

15 List of Tables 3.1 Summary of important dimensions of the robot Summary of good working combination of values for the different parameters of the active localization algorithm Summary of advantages of the different exploration strategies Summary of disadvantages of the different exploration strategies A.1 Summary of the results for the clockwise experiment. Absolute is the measured error in real. Calculated is the data provided by the odometry. 90 A.2 Summary of the results for the counterclockwise experiment A.3 Summary of the results for the counterclockwise experiment A.4 Summary of the configuration of the AMCL B.1 Listing of the important parameters and their values for the configuration of the base local planner B.2 Summary of the parameters for the local costmap in context of the active exploration task B.3 Summary of the parameters for the global costmap in context of the active exploration task B.4 Summary of the important parameters and their chosen values for the costmap parameters, which are the same for the global and the local costmap C.1 Summary of x- and y-coordinates to spawn robot at different locations vii

16 Chapter 1 Introduction Full or partial autonomous systems and mobile robots become more and more part of our daily life. They help us to get into small parking slots with our cars, cut the grass in the garden or clean the floor at home. Two examples for these systems and applications are shown in figure 1.1. But there exist many more systems. (a) An interactive (b) Autonomous floor cleaner of the company Vorwerk [2] butler for home by Frauenhofer IPA [1] Figure 1.1: Examples for mobile robots and full or partial autonomous systems in our daily life Furthermore, mobile robots will also be used in more complex environments for mapping and other applications - for example in old abandoned mines [3] or disaster and accidents locations [4]. There it would be dangerous for humans. Also, competitions for different applications using mobile robots exist. One of these is the RoboCup Rescue competition [3]. Its goal is to develop robots with different skills like mapping, planning, looking for trapped victims and other tasks in this context [5]. Figure 1.2 shows some examples for robots, which can help at disaster locations or old mines. 1

17 Chapter 1. Introduction (a) Groundhog robot for mapping old mines [3] 2 (b) A rescue robot in action at RoboCup 2009 [6] Figure 1.2: Examples for mobile robots to be used in complex environments Another popular example to handle different tasks with autonomous systems in complex environments is the DARPA Grand Challenge in 2005 and DARPA Urban Challenge in In 2005, the teams had to develop an autonomous car, which can drive 132 miles through the desert without any human interaction [7, 8]. The goal for the teams of the challenge in 2007 was to develop autonomous cars, which can interact with other unmanned or manned cars in traffic in an urban environment [8]. Figure 1.3 illustrates two vehicles, taking part in the challenges. (a) Vehicle of the winner team of the DARPA Grand (b) Two vehicles at an intersection at the DARPA Challenge 2005 [9] Urban Challenge 2007 [8] Figure 1.3: Examples for vehicles of the DARPA Grand/Urban Challenge in 2005 and Motivation The problems of localization, perception of the robot and automatic model building are central questions in mobile robotics [10]. How to determine the absolute pose of the robot, if no system is available, which is able to solve the problem? What is the best

18 Chapter 1. Introduction 3 way to explore an unknown environment? Can changes be detected in the environment? Why exploration is very important can be illustrated with some applications. Thinking of the situation, where an area is contaminated, it would be very dangerous for humans to be there. In this case, a robot, which is able to build a representation of the area for further rescue plans is very helpful and to put the robot back to execute this plan, requires a localization capability, too. An other example is planetary exploration. Currently, the Mars is checked on the existence of water by a robot. What is the best way to get to the targets where the robot has to move to and what happens if the environment changes? Further, change detection is very interesting, too. You can think of the further development of existing alarm and warning devices. An automatic surveillance of your flat or house, by a mobile robot, could automatically (maybe even unnoticed) call the police, if a burglar succeeds to break in. The problems of automatic model building and localization are widely discussed in research. Simultaneous Localization And Mapping (SLAM) addresses a solution for building a model of the environment and solves the localization problem at the same time. But the SLAM algorithms have two key problems in context of exploration. The first is that SLAM algorithms yield to wrong localization results, due to a slow changing environment and secondly, the SLAM algorithms do not address the problem, where to move the robot to gather information about the environment, what is also called sensor placement [10]. Therefore, this master thesis seeks to actively contribute to this discussion. How to resolve the key problems of SLAM for autonomous robots in dynamic environments and non-perfect sensors? How can the algorithms and concepts for change detection in remote sensing be adapted for a mobile robot? 1.2 Scope of Thesis The problem of exploration can be divided into two subproblems. The first problem is to explore the environment of a given map to determine the pose of the robot, if this pose is a-priori unknown. This is called active localization. The second problem is to build a map efficiently for a robot unknown environment and track its position at the same time. First of all, a method for the active localization using a given map (e.g. floor plan) has to be implemented. In this context, localization means that the robot finds its pose by exploring the environment. The representation of the map could be an occupancy grid map or any other kind of representation. Secondly, the exploration in an unknown environment has to be realized. Different strategies have to be discussed, implemented by means of simulation and have to be evaluated. One of these strategies has to be

19 Chapter 1. Introduction 4 transferred to a mobile robot (Volksbot RT3) as a real application. Finally, the previous results of the active exploration have to be used to perform a change detection in an unknown environment. 1.3 Structure of Thesis The thesis is structured as follows: Chapter 2 - Theory and Background: This chapter describes the important concepts and algorithms in robotics, which are required to solve the problems of the thesis. First of all, the motion of a robot is described. Due to uncertainties in motion, different probabilistic filters will be discussed to apply measurements to handle the uncertainties in motion. After that, a possible way to represent the environment, using occupancy grid maps, is shown in order to introduce and apply SLAM. Next, the two subproblems of the exploration, the active localization and the active exploration will be discussed and existing solutions to those problems will be presented. Last but not least, the problem of change detection of the environment will be presented and solved with a simple classification-based criterion. Chapter 3 - Sensors and Software: This chapter describes the used robot (Volksbot RT3), the sensors (odometry and laser scanner) and introduces the coordinate systems of the robot and the sensors. Further, the Robot Operating System and the simulation tool Gazebo are introduced, which are used to implement the solutions for the underlying problems of the thesis. Chapter 4 - Scenarios, Implementation and Experiments: This chapter describes the implementation of the methods described in chapter 2 and which experiments will be executed. Chapter 5 - Results and Evaluation: This chapter summarizes and shows the results of the different experiments and evaluates the solutions for the underlying problems. Chapter 6 - Conclusion and Outlook: The last chapter summarizes the thesis and gives an outlook, what can be done in further work and how the implemented algorithms can be improved.

20 Chapter 2 Theory and Background The upcoming chapter presents the important concepts and methods for this thesis. First of all, a noise free motion model to convert the motor ticks of the odometry to a 2D pose of the robot is described. Then, different probabilistic filter techniques will be discussed to solve the localization problems. In addition, a possible representation of the environment and a way to create this is shown in this chapter. Further, two different exploration problems will be described. The first is the active localization, to get the robot s pose by exploring the environment, whereas the second is the active exploration. Hereby, different exploration strategies will be introduced to learn occupancy grid maps of an unknown environment with a single robot. Last but not least, a simple criterion to detect changes will be introduced. 2.1 Odometry-based Motion Model The robot s differential odometry provides the information, how many ticks the left and the right motor have done between time steps t-1 and t. The origin of the coordinate system is in between the two wheels for t 0. Figure 2.1: Illustration of the motion: robot moves forward and turns left 5

21 Chapter 2. Theory and Background 6 The last known pose of the robot is given by x t 1, y t 1 and θ t 1. The width between the centers of the two wheels w is known. Also, the motor ticks between t-1 and t of the motors are given by ticks left for the left motor and ticks right for the right motor. It is assumed that the robot moves forward and performs a left turn. For that movement, the right motor rotates with a higher rate than the left motor. The principle motion is shown in figure 2.1. L and R (see: figure 2.1) can be determined as follows [11, 12]: R = α (R + w) (2.1) L = α R (2.2) α and R are unknown in these both equations. The subtraction of equations 2.1 and 2.2 yields to Therefore and R L = α w (2.3) α = R L w R = L α (2.4) (2.5) R is only computable if α is not zero. If α is zero, the robot will move straight forward and the point c would be in infinity [12]. This case will be described later in this section. For the upcoming equations, α is assumed not be zero. L and R can be also expressed as follows: L = ticks left c mm (2.6) R = ticks right c mm (2.7) c mm describes the factor to get the driven distance in millimeters from the motor ticks. To calculate the new pose of the robot figure 2.1 indicates that the robot moves around the center of movement c. following equation [11, 12]: ( cx c y ) The Cartesian coordinates of c will be calculated by the = ( xt 1 y t 1 ) ( ) (R + w 2 ) sin(θt 1 ) cos(θ t 1 ) (2.8) Using this center of movement, the pose at time stamp t (x t,y t and θ t ) can be calculated as follows [11, 12]: ( xt y t ) = ( cx c y ) ( ) + (R + w 2 ) sin(θt 1 + α) cos(θ t 1 + α) (2.9)

22 Chapter 2. Theory and Background 7 θ t = (θ t 1 + α)mod(2π) (2.10) The modulo operation in equation 2.10 is necessary to be sure that the heading is within a range of [ 0; 2π [. If the ticks of the left motor equals the ticks of the right motor, the robot moves straight forward (i.e. α = 0). Therefore, the calculation of the new pose is quiet simple: x t = x t 1 + l cos(θ t 1 ) (2.11) y t = y t 1 + l sin(θ t 1 ) (2.12) θ t = θ t 1 (2.13) The unknown variable l is given by (only valid for the case: ticks left equals ticks right ): l = ticks left c mm = ticks right c mm (2.14) The shown motion model is a noise free model. In practice, systematic and nonsystematic errors will add noise which will result in variations from the calculated pose. 2.2 Probabilistic Filter - Bayes Filter Probabilistic filters are filters, which represent the current true state at time t, using a belief b(x t ) and can consist of any variable that has to be estimated. All of those filter techniques have the Markov assumption first order as precondition. This assumption means that the current state x t only depends on the previous state x t 1, the current measurement z t and the control data u t. This is illustrated in figure 2.2. Figure 2.2: Illustration of the Markov assumption as a dynamic Bayesian network In general, the Bayes filter consists of two steps: Prediction step: During this step the control data, e.g. data of the odometry, will be used to predict the new belief. The equation for the prediction step is [4,

23 Chapter 2. Theory and Background 8 p.27]: bel(x t ) = p(x t u t, x t 1 ) bel(x t 1 ) dx t 1 x t 1 p(x t u t, x t 1 ) bel(x t 1 ) (2.15) Correction step: After the prediction step, the current measurement z t will be used to correct the state. The equation for the correction step is [4, p.27]: bel(x t ) = η p(z t x t ) bel(xt ) (2.16) The correction step is also called as update step. η is a normalization value, to normalize bel(x t ). The discrete version of the Bayes filter is described as pseudocode in algorithm 1. Accordingly to the Markov assumption, the input of the Bayes filter is the belief b(x t 1 ), the current control data u t and the current measurement z t. Algorithm 1 Recursive discrete Bayes filter (source: [4]) 1: procedure BayesFilter(bel(x t 1 ), u t, z t ) 2: for all x t do 3: bel(xt ) = x t 1 p(x t u t, x t 1 bel(x t 1 ) Prediction step from equation : bel(x t ) = η p(z t x t ) bel(xt ) Correction step from equation : end for 6: return bel(x t ) 7: end procedure The Bayes filter returns the a-posteriori belief for time stamp t Kalman Filter The Kalman filter is derived from the Bayes filter and provides a concrete algorithm for estimating a state. The Kalman filter was published by R. E. Kalman in 1960 and solves the problem of prediction in state space for linear systems [13]. For the Kalman filter, an additional assumption, to the Markov assumption first order, will be made, namely that all believes will be represented by zero-centered Gaussian distributions: bel(x t ) N (0, σ 2 )

24 Chapter 2. Theory and Background 9 All equations in the following sections are given for the case that the state vector containing the pose of the robot x x t = y θ and the measurement contains distance and a bearing angle: ( ) d z t =. α Linear and Time Discrete Kalman Filter Given is a linear and time discrete system with its system equation [4, p.41]: x t = A t x t 1 + B t u t + ɛ t (2.17) x t and x t 1 are the state vector at time t and t 1. u t is a vector, containing the current control data. A t describes the system dynamics and the matrix B t adds the control data. The last term ɛ t is the noise. The noise is a zero-centered Gaussian with covariance matrix R t. This describes the uncertainties of the state transition [4]. The prediction of the state vector is given by: x t = A t x t 1 + B t u t (2.18) The uncertainties of the system have to be predicted, too. The prediction of the uncertainties is given by [4]: Σ t = A t Σ t 1 A T t + R t (2.19) Now, a measurement is available and the predicted state can be updated. The measurement equation of the linear and time discrete system is given by: z t = C t x t + δ t (2.20) δ t is the noise of the measurement and is a zero-centered Gaussian with covariance matrix Q t. Using a recursive linear estimation, the state vector can be estimated. The result for x t is [4]: x t = x t + K t (z t C t x t ) (2.21) The difference between z t and C t x t is the innovation of the update step. The matrix K t is a currently unknown matrix and is called Kalman gain and specifies how much the estimation of the state depends on the current measurement. The Kalman gain matrix

25 Chapter 2. Theory and Background 10 is given by [4]: K t = Σ t C tt (C t Σ t C t T + Q t ) 1 (2.22) Now, an algorithm for the Kalman filter can be defined. The algorithm requires the mean of the last state µ t 1, the covariance matrix Σ t 1, the control data u t and the measurement data z t. Algorithm 2 Algorithm of the linear and time discrete Kalman filter (source: [4, p.42]) 1: procedure KalmanFilter(µ t 1, Σ t 1, u t, z t ) 2: µ t = A t x t 1 + B t u t 3: Σt = A t Σ t 1 A T t + R t 4: K t = Σ t C tt (C t Σ t C T t + Q t ) 1 5: µ t = µ t + K t (z t C t µ t ) 6: Σ t = (I K t C t ) Σ t 7: return (µ t, Σ t ) 8: end procedure The algorithm returns the posterior belief, represented by the mean and the covariance of the Gaussian distribution Extended Kalman Filter (EKF) First Order In most cases, the system and measurement equations are nonlinear. The given system equation is denoted by g(u t, x t 1 ) and is nonlinear. The predicted system state is given by [4, p.54]: x t = g(u t, x t 1 ) + ɛ t (2.23) The measurement equation is denoted by h(x t ) and is nonlinear, too. Therefore, the measurement z t is given by [4, p.54]: z t = h(x t ) + δ t (2.24) ɛ t and δ t are the additive noise of the system and measurement processes. To handle those nonlinear cases, the extended Kalman filter 1. order linearizes the nonlinear equations, using the Taylor series expansion and breaking up this expansion after the first term. Linearizing g(u t, x t 1 ), using the Taylor series expansion and omitting higher order terms, leads to [14]: g(u t, x t 1 ) g(u t, x t 1 ) + g(u t, x t 1 ) x t 1 x t 1 (2.25) The system equation depends on more than one variable that is part of the state vector x t 1. Therefore, the matrix G t will be introduced. G t is the Jacobian matrix of

26 Chapter 2. Theory and Background 11 g(u t, x t 1 ) with respect to the current state x t. In particular, that means, g(u t, x t 1 ) will be approximated by its mean of the last known state µ t 1 and u t [4, p.58]: g(u t, x t 1 ) g(u t, µ t 1 ) + G t (x t 1 µ t 1 ) (2.26) The system function contains single equations for every component of the state vector - in particular for the state vector containing x, y and θ: g 1 (u t, x t 1 ) g(u t, x t 1 ) = g 2 (u t, x t 1 ) (2.27) g 3 (u t, x t 1 ) Therefore, the Jacobian matrix G t is: G t = g 1 (u t,x t 1 ) x g 2 (u t,x t 1 ) x g 3 (u t,x t 1 ) x g 1 (u t,x t 1 ) y g 2 (u t,x t 1 ) y g 3 (u t,x t 1 ) y g 1 (u t,x t 1 ) θ g 2 (u t,x t 1 ) θ g 3 (u t,x t 1 ) θ (2.28) The same will be done for the measurement equation, containing single equations for every measured variable - in particular for measuring a distance and a bearing angle, h(x t ) is: ( ) h1 (x t ) h(x t ) = (2.29) h 2 (x t ) The corresponding Jacobian matrix H t is: H t = ( h1 (x t) x h 2 (x t) x h 1 (x t) y h 2 (x t) y h 1 (x t) θ h 2 (x t) θ ) (2.30) Therefore, the linearized measurement function is given by [4, p.58]: h(x t ) h(µ t ) + H t (x t µ t ) (2.31) Analogously to the linear and time discrete Kalman filter, the pseudocode for the EKF can be defined. The algorithm requires, like the Kalman filter before, the mean of the last state µ t 1, the covariance matrix Σ t 1, the control data u t and the measurement data z t. The algorithm returns the posterior belief, represented by the mean and the covariance of the Gaussian distribution.

27 Chapter 2. Theory and Background 12 Algorithm 3 Algorithm of the Extended Kalman Filter 1. order (source: [4, p.59]) 1: procedure ExtendedKalmanFilter(µ t 1, Σ t 1, u t, z t ) 2: µ t = g(u t, µ t 1 ) 3: Σt = G t Σ t 1 G T t + R t 4: K t = Σ t H tt (H t Σ t H T t + Q t ) 1 5: µ t = µ t + K t (z t h( µ t )) 6: Σ t = (I K t H t ) Σ t 7: return (µ t, Σ t ) 8: end procedure Additional Kalman Filter Implementations Also, an iterative version of the EKF first order exists, which produces more accurate results then the non-iterative version of the EKF. An introduction to the iterative version of the EKF first order is given in [15]. In addition, an EKF second order exists. The second derivative of the non-linear functional model will be calculated, using the Taylor series expansion and breaking at the second term. A description can be found in [15]. Last but not least, there is an additional Kalman filter approach for non-linear systems. This one is called Unscented Kalman filter (UKF). The UKF tries to linearize the nonlinear functional model via an unscented transformation, instead of using the Taylor series expansion. A description of this filter is given in [4] and [16] Particle Filter The particle filter is an other derived filter of the Bayes filter and provides an algorithm for estimating the state, without the assumption of using a normal distribution for the beliefs. Every multi modal distribution can be handled by this nonparametric filter. The true state is estimated by a set of particles χ t. A particle x [i] t represents one possible state of the true state. The index i addresses the particle of the particle set χ t Algorithm The principle algorithm of the particle filter is given as pseudocode in algorithm 4. This algorithm requires the last known state, represented by a set of particles χ t 1, the current control data u t and the current measurement z t. The particle filter iterates over the whole set of particles. At first, the prediction step will be applied to the data. Due to the sampling of a possible pose under the condition of the control data, uncertainties are handled too (approximates bel(x t ) from equation 2.15). After that, the measurement will be used to proceed the correction step. The correction step returns a likelihood, which describes how well a measurement corresponds

28 Chapter 2. Theory and Background 13 Algorithm 4 General algorithm of the particle filter (source: [4, p.98]) 1: procedure ParticleFilter(χ t 1, u t, z t ) 2: χ t = χ t = 3: for m = 1 to M do M is the count of particles p(x t u t, x [m] t 1 ) Prediction step (approximates bel(x t )) = p(z t x [m] t ) Apply measurement 6: χ t = χ t + x [m], w [m] 4: sample x [m] t 5: w [m] t t t 7: end for 8: for m = 1 to M do 9: draw i with probability w [i] t Resampling 10: add x [i] t 11: end for to χ t Approximates bel(x t )) 12: return χ t 13: end procedure to the real world, according to the particle x [i] t. In the next step, the particles will be added with their corresponding weight to the set of particles. Finally, the particle set will be resampled. In this step, particles with low weights will be likely to be removed and particles with high weights will be likely to be duplicated. This approximates the bel(x t ) from equation Techniques for resampling will be discussed in the upcoming section. The concrete implementation of the different steps depends on the used motion model, sensor model and the resampling technique. Some possible models are discussed in the upcoming sections of this chapter Resampling Techniques Resampling is the process of drawing M particles from χ t with replacement. Particles with high weights are more likely to be still part of the particle set and the low weighted particles are more unlikely to be part of the set. This is also known as importance sampling [4]. After the resampling, all particles have the same weights and approximately distributed to bel(x t ) from equation 2.16 [4]. The resampling can be done, using different techniques. Two common techniques are described in the following. Resampling Wheel The resampling wheel is one possible technique for resampling and works as follows: At first a random index and a random offset will be chosen. Up from the index, the offset will be added. If the offset passes one weight, the index will be increased. This

29 Chapter 2. Theory and Background 14 will be done, until M particles are drawn. The described functionality is illustrated in figure 2.3. Figure 2.3: Illustration how the resampling wheel selects the particles, as proposed in [17] A pseudocode for this technique is given in algorithm 5. The algorithm requires the current particle set and weights as input. Algorithm 5 Algorithm of the resampling wheel method, based on [17] 1: procedure ResamplingWheel(χ t, w t ) 2: χ new = 3: index p(index) Index drawn from uniform distribution 4: offset = 0.0 5: for i to M do 6: offset = offset + p(n) Uniform distribution p(n) with 0 < n < w max 7: while offset > w t (index) do 8: offset = offset w t (index) 9: index = index + 1 mod M M is the count of particles in χ t 10: end while 11: χ new = χ new + χ t (index) 12: end for 13: return χ new 14: end procedure The resampling wheel returns the new set of particles χ new. Low Variance Sampler The low variance sampler is another technique for resampling. The idea of the algorithm is to select the particles based a sequential stochastic process [4]. The algorithm draws a random number and samples from the set of particles with a probability proportional to the weight of the particle according the random number [4]. The algorithm

30 Chapter 2. Theory and Background 15 provides this as follows: A random number r will be sampled from a uniform distribution. After that the particles will be selected by adding the fixed amount of M 1 to r [4]. This is shown in figure 2.4. Figure 2.4: Illustration how the low variance sampler selects the particles (source: [4, p.111] A pseudocode for the low variance sampler is given in algorithm 6. requires the current set of particles and their corresponding weights. The algorithm Algorithm 6 pseudocode of the low variance sample algorithm (source: [4]) 1: procedure LowVarianceSampler(χ t, w t ) 2: χ t = 3: r p(r) r drawn from uniform distribution 0 r M 1 4: c = w [1] t 5: i = 1 6: for m = 1 to M do 7: U = r + (m 1) M 1 8: while U c do 9: i = i : c = c + w [i] t 11: end while 12: add x [i] t 13: end for to χ t 14: return χ t 15: end procedure The low variance sampler algorithm returns the new set of particles and has three advantages [4]: Covers the space of samples more systematic If all weights of the samples are the same, the result is the following: χ t = χ t The algorithm just has a linear complexity of O(m) Particle Deprivation A problem, which may arise from the resampling process is called particle deprivation or particle depletion in literature. Particle deprivation describes the effect that particles, which are close to the true state, are removed of the particle set. This happens during

31 Chapter 2. Theory and Background 16 the resampling process, if a series of random numbers are generated and these random numbers force only low weighted particles to be left in the particle set. Some algorithms try to handle this problem by adding random particles to the current set of particles after a defined decade [4], e.g. the Adaptive Monte Carlo Localization. The problem of the particle deprivation can not be avoided by a general solution. The effect can be reduced, e.g. if the resampling will not be performed after every update step of the filter. Also, the count of used particles can deal with the problem. The more particles are used, the higher is the probability that the particles, estimating the true state, are still alive after resampling. The drawback of a high amount of particles is an increasing computational time, which may cause problems to real time applications. But the respective solution depends on the current scenario and its complexity. 2.3 Representation of the Environment - Occupancy Grid Map Occupancy grid maps are one possible way to represent the environment. This kind of map is a location-based map and rasters the world into discrete cells with a set size (resolution with [ m px ]). Using occupancy grid maps, the real world will be represented by three different kinds of space: Free space: Free space is the space in the real world, where the robot can move without colliding with an obstacle. Free space is represented by white pixels in a visualization of an occupancy grid map. Occupied space: Occupied space describes the space in the map where obstacles like walls or doors are in the real world and the robot can not pass without collision. Visualizing this kind of space in an occupancy grid map, the pixels are black. Unknown space: Unknown space describes the space in a map, if it is not explored and no information about its state is available. This kind of space is represented as gray pixels for a visualization of an occupancy grid map. This case is helpful for exploring an unknown environment. Figrue 2.5 shows an example of a recorded occupancy grid map with the different types of space. The white space is free, the black space are obstacles, for this space is occupied and the light gray colored space is unexplored. Internal, an occupancy grid map can represented as an one or two dimensional array (representing the cells) with the corresponding value, if the cell is occupied or not. For the case that a cell is not explored the value of the cell is -1.

32 Chapter 2. Theory and Background 17 Figure 2.5: Example of an occupancy grid map (source: [4]) An other kind of the representation of the environment are semantic maps. A good overview of this kind of representation, for the 3D case, can be found in [3, p.170ff]. 2.4 Adaptive Monte Carlo Localization The adaptive Monte Carlo localization (AMCL) is a method to localize a robot in a given map. It is a concrete implementation of the particle filter with some modifications. Also, it is an adaptive algorithm of the Monte Carlo localization, because the count of used particles is not fixed. The decision how many particles will be used is based on KLD-Sampling (Kulback-Leibler-Divergence). KLD-Sampling is described in [18] or [4]. Algorithm 7 describes the AMCL. It requires the set of particles of the last known state χ t 1, the control data u t for prediction and the measurement data z t and the map m for the update. The algorithm returns the new state, represented by a set of particles. An advantage of the AMCL is, that this filter implementation is able to solve the global localization problem, the kidnapped robot problem, as well as localization tracking. Notice, the AMCL can use an arbitrary resampling technique. Due to that, this will not be discussed in this case. For the different resampling techniques, take a look on section A second advantage of the AMCL is, that it is able to recover from localization errors by adding random particles to the set χ t after a specified decade (see: lines 13 and 14) Motion Model for the Prediction Step In section 2.1, a noise free motion model has been described, where the pose could be calculated from raw data (motor ticks or also called control data) of a differential odometry. The following model describes a noisy model for the odometry.

33 Chapter 2. Theory and Background 18 Algorithm 7 Adaptive variant of Monte Carlo Localization (source: [4, p.258]) 1: procedure AMCL(χ t 1, u t, z t, m) 2: static w slow, w fast 3: χ t = χ t = 4: for m = 1 to M do Iterate over all particles 5: x [m] t 6: w [m] t 7: χ t = χ t + x [m] t = SampleMotionModelOdometry(u t, x [m] = MeasurementModel(z t, x [m] t, w [m] t 8: w avg = w avg + 1 M ẇ[m] t 9: end for 10: w slow = w slow + α slow (wavg w slow ) 11: w fast = w fast + α fast (wavg w fast ) 12: for m = 1 to M do { 13: with probability max 0.0, 1.0 w fast t 1 ), m) Beam model or likelihood fields w slow } 14: add random pose to χ t 15: else 16: draw i {1,..., N} with probability w [m] t 17: add x [i] t to χ t 18: endwith 19: end for 20: return χ t 21: end procedure do As mentioned before, AMCL is a concrete implementation of the particle filter. Thus, a sampling algorithm to model the true pose of the robot is required. Given is the last known pose x t 1. In addition, the current pose estimate of the odometry is given by x t. Every motion between two poses x t 1 and x t consists of a translational and two rotatory parts (see: figure 2.6). Both the translational as well as the two rotatory parts are noisy and effect each other. Therefore, this motion model calculates an better approximation of the pose x t as like the noise free motion model before. Figure 2.6: Illustration of the translational and rotatory parts of a robots motion between two poses x t 1 and x t using an odometry-based model (source: [4, p.133]) The pseudocode for the motion model is given in algorithm 8. The motion model requires the control data u t and the last pose known x t 1 = (x t 1, y t 1, θ t 1 ) as input. The

34 Chapter 2. Theory and Background 19 control data u t contains the last pose ( x t 1, ȳ t 1, θ t 1 ) and the current pose ( x t, ȳ t, θ t ) of the robot. Algorithm 8 Sampling algorithm for an odometry-based motion model (source: [4]) 1: procedure SampleMotionModelOdometry(u t, x t 1 ) 2: δ rot1 = atan2(ȳ t ȳ t 1, x t x t 1 ) θ t 3: δ trans = ( x t 1 x t ) 2 + (ȳ t 1 ȳ t ) 2 4: δ rot2 = θ t θ t 1 δ rot1 5: ˆδrot1 = δ rot1 sample(α 1 δrot α 2 δtrans) 2 6: ˆδtrans = δ trans sample(α 3 δ 2 trans + α 4 δ 2 rot 1 + α 4 δ 2 rot 2 ) 7: ˆδrot2 = δ rot2 sample(α 1 δ 2 rot 2 + α 2 δ 2 trans) 8: x = x + ˆδ trans cos(θ + ˆδ rot1 ) 9: y = y + ˆδ trans sin(θ + ˆδ rot1 ) 10: θ = θ + ˆδ rot1 + ˆδ rot2 11: return x t = (x, y, θ ) 12: end procedure At first, the difference between the two poses will be calculated. This difference describes the motion of the robot in its translational and rotatory parts. After that, a random noise will be added to the different parts. The random noise is sampled from a Gaussian distribution, with the translational respectively the rotatory parts as mean and variance given by different factors α 1 to α 4. The approximated true pose of the robot, will be calculated using polar attaching Measurement Models for the Update Step In this section, two different measurement models for the update step will be described. Both measurement models are for a laser scanner. The first one is an approximative physical model, the beam model [4, p.153]. Likelihood fields are the second model, which try to handle limitations of the beam model [4, p.169]. Beam Model This model is a mixture of four different types of measurement errors [4, p.153]. The four different types are: Measurement noise: No measurement is perfect in the real world. zt k is the measured distance and zt k is the true distance to the measured object. Using occupancy grid maps, zt k can be determined using ray casting [4, p.154]. Every measurement has a noise, due to a limited resolution of the scanner or atmospheric effects and many more [4, p.154]. The noise can be modeled using a Gaussian

35 Chapter 2. Theory and Background 20 distribution with z k t as mean and σ hit as standard deviation. Therefore, the probability for the measurement is given by [4, p.155]: p hit (zt k N (zt k ; zt k, σhit 2 x t, m) = ), if 0 zk t z max 0, otherwise (2.32) Unexpected objects: A robot operates in dynamic environments. In contrast, the used map for the localization is static. Therefore, non mapped or unmodeled objects produces unexpected short ranges during a measurement. for that are people moving in front of the robot. An example For the beam model, these unexpected objects are treated as additional sensor noise. Due to the fact that the likelihood of measuring those unexpected objects decreases with the range, this can be modeled by an exponential distribution [4, p.155]: p short (zt k λ short e λ short zt k, if 0 z k t zt k x t, m) = 0, otherwise λ short is an intrinsic parameter of the beam model [4, p.155]. (2.33) Sensor failures: Sensor failures of the laser scanner arise from measured objects with a black, light absorbing surface. maximum range value z max [4, p.156]. follows: p max (zt k 1, if z = z max x t, m) = 0, otherwise In this case a laser scanner returns the This sensor failures can be modeled as (2.34) Random measurement: Additional to the described errors before, sensors produce errors, which can not explained by concrete effects. This will be modeled by a uniform distribution in the range of [0; z max : 1 p rand (zt k z x t, m) = max, if 0 zt k z max 0, otherwise (2.35) All single distributions have to be normalized. To get the combined density p(zt k x t, m), which represents the likelihood for a single laser beam k, all distributions are mixed. Every distribution gets a weight (z hit, z short, z max and z rand ) and the sum of the weights is one. Therefore the combined density is: p(z k t x t, m) = z hit p hit (z k t x t, m) z short p short (z k t x t, m) z max p max (z k t x t, m) z rand p rand (z k t x t, m) (2.36)

36 Chapter 2. Theory and Background 21 p(z k t x t, m) from equation 2.36 is illustrated in figure 2.7. Figure 2.7: Possible density of the mixture distribution from equation 2.36 (source: [4, p.157]) The beam model calculates the likelihood for every beam and returns the overall likelihood for the complete scan. Algorithm 9 shows the pseudocode for the model. As input, the beam model requires the current measurement z t, the current pose x t and the occupancy grid map m. The algorithm returns the overall likelihood for the measurement z t (laser scan). Algorithm 9 Beam model (source: [4]) 1: procedure BeamModel(z t, x t, m) 2: q = 1 3: for every beam do 4: compute zt k for the measurement zt k using ray casting 5: p = z hit p hit (zt k x t, m)+z short p short (zt k x t, m)+z max p max (zt k x t, m)+z rand p rand (zt k x t, m) 6: q = p q 7: end for 8: return q 9: end procedure An advantage of this model is, that dynamics in the environment are modeled by a single distribution (z short ). In contrast, the model has two disadvantages. The first one is the high computational costs for calculating the likelihood for every beam, because the ray-casting algoritm to calculate zt k has high computational costs. This disadvantage can be handled by reducing the calculation only for every k-th equally beam, which has a benefit at all. The benefit is that it can prevent from suboptimal results, if dependencies between adjacent measurements are strong [4, p.167]. Also, the beam model has a problem with environments with many small obstacles (e.g. conference room), which can cause wrong approximations of the pose of the robot and the localization process may fail. In [4] on page 160, an additional algorithm is described to learn the parameters (weights

37 Chapter 2. Theory and Background 22 of the different distributions from equation 2.36) of the beam model from the measurement data. An important precondition for this learning algorithm is a good initialization of the parameters σ hit and λ short [4]. Likelihood Fields As described before, the beam model has some drawbacks. The likelihood field tries to handle these and is a non physical based model. It is a mixture of three different types of errors [4, p.169], instead of four types compared to the beam model. The three different types are: Measurement noise: The measurement noise describes errors of the measurement, due to production errors, environmental effects and so on. represented by a Gaussian distribution: This noise is p hit (z k t x t, m) = N (d, σ 2 hit ) (2.37) The variable d denotes the distance between the endpoints (x z k, y t z k) of the laser t beam zt k and the nearest object in the map. σ hit is the standard deviation of the measurement. Sensor failures: This are maximum range readings from the laser scan due to erroneous measurements, as like for the beam model. These are modeled as shown in equation Random measurement: Additional to the other errors, unexplainable errors are produced by the laser scanner, too. These errors are modeled by a uniform distribution as like for the beam model and are given by equation Using the beam model, a ray-casting algorithm is used to calculate the cell in the map, where the laser beam hits. This ray-casting operation is very inefficient. In contrast, here the endpoint of the beam will be calculated. The endpoint, expressed as Cartesian coordinates in the robots coordinate system, of a single laser beam zt k can be calculated as follows [4, p.169]: ( ) xz k t = y z k t ( xr y r ) ( ) cos(θr ) sin(θ r ) + sin(θ r ) cos(θ r ) ( xk,sens y k,sens ) + z k t ( ) cos(θr + θ k,sens ) sin(θ r + θ k,sens ) (2.38) where zt k the depth of the measured laser beam, (x k,sens, y k,sens, θ k,sens ) parametrized the transformation from the scanner coordinate system to the robot coordinate system

38 Chapter 2. Theory and Background 23 and x r, y r and θ r describes the current pose of the robot. This calculation of the endpoint is more efficient and is an advantage adverse to the beam model. The pseudocode for the likelihood field model is given in algorithm 10. The algorithm requires, as like the beam model, the current measurement z t, the current pose x t and the occupancy grid map m. likelihood for the current laser scan. Algorithm 10 Likelihood field model (source: [4]) Also, the likelihood field algorithm, returns the overall 1: procedure LikelihoodFieldModel(z t, x t, m) 2: q = 1 3: for every beam k do 4: if zt k z max then 5: x z k = x t r + x k,sens cos(θ r ) y k,sens sin(θ r ) + zt k cos(θ + θ k,sens ) 6: y z k = y t r + y k,sens cos(θ r ) x k,sens sin(θ r ) + zt k sin(θ + θ k,sens ) 7: dist = min x,y { (x z k x ) 2 + (y t z k y ) 2 x, y occupied in m} t 8: q = q (z hit N (dist, σ hit ) + z random z max ) 9: end if 10: end for 11: return q 12: end procedure The calculation of the nearest distance in line seven of the pseudocode 10 is most expensive calculation [4, p.172]. Therefore, it is useful to pre-compute the likelihood field for a given map before starting the filtering process. After that, the search for the nearest distance to an endpoint is only a look up operation with constant computational costs. Figure 2.8 shows an example of a computed likelihood field. The darker a specific location in the map, the less likely is the location to perceive the object in real world. The light gray painted areas are unexplored. Figure 2.8: Example of a pre-computed likelihood field of an occupancy grid map. (source: [4, p.173])

39 Chapter 2. Theory and Background Rao-Blackwellized Particle Filter Occupancy Grid SLAM Rao-Blackwellized Particle Filter Occupancy Grid SLAM describes a possible way to solve the SLAM problem, by learning an occupancy grid map. Classical SLAM approaches create feature based maps, e.g. using landmarks [4]. SLAM is called Simultaneous Localization And Mapping. It solve the localization problem and building a representation of the a-priori unknown environment. The Rao-Blackwellized particle filter estimates the joint posterior about the map and the complete trajectory, based on all measurements and control data. For SLAM, the Rao-Blackwellized particle filter uses the following factorization [14]: p(x 1:t, m z 1:t, u 1:t 1 ) = p(m x 1:t, z 1:t ) p(x 1:t z 1:t, u 1:t 1 ) (2.39) Using this factorization, the trajectory can be calculated and further the map, using the known poses from the trajectory. The Rao-Blackwellized particle filter occupancy grid SLAM works as follows, if new control data u t from the odometry and a new measurement z t from the laser scanner is available [14]: 1. Determine the initial guess x (i) t, based on u t and the pose, since the last filter update x t 1 has been estimated. 2. Perform a scan matching algorithm based on the map m (i) matching fails, the pose x (i) t model, otherwise the next two steps will be performed. t 1 and (i) x t. If the scan of particle i will be determined according to a motion 3. If the scan matching is successfully done, a set of sampling points around the estimated pose ˆx (i) t of the scan matching will be selected. Based on this set of poses, the proposal distribution will be estimated. 4. Draw pose x (i) t of particle i from the approximated Gaussian distribution of the improved proposal distribution. 5. Perform update of the importance weights. 6. Update map m (i) of particle i according to x (i) t and z t. A more detailed introduction to learn occupancy grid maps and the Rao-Blackwellized particle filter can be found in [4], [19] and [14].

40 Chapter 2. Theory and Background Path Planning Path planning is one general task of navigation. There are different methods to plan a path in context of minimal costs. All presented algorithms are shown for the use on occupancy grid maps, but they also work with other kinds of representations. For occupancy grid maps, just movements between the centers of the grid cells are allowed Non-optimal Path Planning At first a non optimal path planner will be discussed. As can be slightly seen, the drawback of the planners is that they do not find the optimal path, in every case. This strategy calculates the costs from a start position to every other possible position. Breadth First Search The breadth first search requires a start cell for the search. This cell will be labeled as visited. Up from this cell, every neighbor in a 8-neighborhood will be checked, if it is free. If the neighbor cell is free, it will be added to a queue of cells, which have to be inspected in next step. After all neighbors are checked, the front element of the queue is removed and inspects its neighbors. If a neighbor is already labeled as visited, this neighbor will not be pushed into the queue. If no cell is left in the queue, all costs from the start to every free cell in the map are determined Optimal Path Planning The next strategies are optimal path planners. These planners find the optimal path from a given position to a goal position, in every case a valid path can be found. This effects that the search space will be drastically reduced, in contrast to the discussed planners before Dijkstra The Dijkstra Algorithm takes the start cell and labels it as visited. Up from this cell all neighbors (in a 4- or 8-neighborhood) are added to a list, which saves already inspected cells but not visited cells yet and calculates the costs g(x). In the next step, the algorithm picks the cell with the minimal costs g(x) and labels it as visited, too. All neighbors of this cell are labeled as inspected, if they have not yet been labeled as visited or inspected. If the goal is reached, the search will break and the minimal costs are known.

41 Chapter 2. Theory and Background A*-Algorithm The main difference between the Dijkstra and A* is that not only the costs g(x) are used to calculate the optimal path, instead an additional heuristic h(x) will be used. Therefore, the cell with the minimum heuristic will be inspected next. Important is that A* only works, if h(x) do not underestimates costs to the goal. A widely used heuristic is the Euclidean distance. The rest of the algorithm does not change. The idea of a heuristic makes the A* algorithm faster then the Dijkstra. It can be seen that the Dijkstra algorithm is a special case of the A* algorithm with h(x) = Additional Methods for Path Planning There are additional method to realize path planning, e.g. greedy algorithm, value iteration, optimal hill climbing and many more. The greedy algorithm works like the A* algorithm with a heuristic, which is not optimal and leads, therefore, to non-optimal results. Value iteration is a Markov Decision Process (MDP) based method and is introduced in [4]. This algorithm calculates for every movement a trade-off function. 2.7 Exploration In general, exploration tries to solve the question: Given what you know about the world, where should you move to gain as much new information as possible? [20]. The exploration problem can be distinguished in: Active localization: In this case, the information to be maximized is the robots pose. Active exploration: In this case,the information to be maximized is the map of the unknown environment Known Environment - Active Localization Active localization is the simplest form of the exploration problem [4, p.575]. The robot tries to find a solution for the question Where I am?, by exploring the environment by its own. In fact, the active localization task consists of two subproblems [21]: global localization, to localize the robot

42 Chapter 2. Theory and Background 27 active navigation, to decide, where to go to gather as much information as possible At the initial step of the active localization, the belief b(x) is given by b(x 0 ) (see: equation 2.40) and the map of the environment where the localization starts to proceed is given by m. Global Localization The first problem, the global localization, can be solved using any filter technique, which can be solved by using any of the above described filter techniques. This approach uses the AMCL. Global localization is a special case of the localization problem. In contrast to the standard localization problem, for the global localization is an approximative pose and its covariance a-priori unknown. A useful way for initializing the AMCL, is to use a uniform distribution. This distribution spreads the particles of the initial belief over the whole map. Therefore, the initial belief of the particle filter is given by [4]: bel(x 0 ) = 1 x (2.40) where x are all poses on the map. Active Navigation First of all, the term of exploration action has to be defined. An exploration action is an executable motion, which is defined in the local coordinate system of the robot and contains how much the robot has to move in x- and y-direction. It is important, that any exploration action a exp can be transformed to a control action u. An example for such an exploration action can be move 1m forward and 2m to the left. The main goal of the active navigation task is to find the next best target and drive to it by maximizing the information about the robots pose, i.e. minimize the expected entropy H(x) of the robots pose. The entropy is defined as [4]: H(x) = p(x) log(p(x))dx (2.41) At the initial situation of the localization process, the belief is a uniform distribution and therefore the entropy is maximal [22]. In general, the minimization of the entropy can be done by maximizing the trade-off between the expected entropy in the future and the costs to get to the next target. The expected entropy of a belief after executing

43 Chapter 2. Theory and Background 28 an action and measuring is defined as follows [4]: H b (x u, z) = B(b, z, u)(x ) log(b(b, z, u)(x )dx (2.42) If the belief B(b,z,u) is concentrated on a single pose, the entropy is minimal [22], which corresponds to the main goal of the algorithm. Now, a set of exploration actions can be defined, which are used to solve the active localization. This set of actions can contain any exploration actions that is executable by the robot. The selection of the next best exploration action from the pre-defined set, minimizing the entropy, follows a greedy approach. To determine the next best exploration action, one particle, i.e. one hypothesis of the localization, of the belief b(x) will be drawn [4]: sample x b(x) (2.43) Using the drawn particle, all possible exploration actions will be applied to search for the next best. At first, the movement of the robot will be simulated. According to uncertainties when moving in the real world, it is not enough only to add the exploration action a exp on the drawn particle. Therefore, the new pose after moving the robot, is sampled under the condition of the old pose and the movement [4]: x p(x x, a exp ) (2.44) Now, the measurement at the new pose x will be simulated. As previously discussed, the measurement process is also overlapped with uncertainties. Therefore, the measurement z has to be sampled from a probabilistic model, too [4]: z p(z x ) (2.45) To reiterate, the approach has the scope to minimize the expected entropy H(x). To check, how likely the exploration action minimize the entropy, the expected entropy H b (x) after applying the exploration action is required. Therefore, the belief b (x) have to be estimated, when applying the movement of a exp and the measurement z. Usually, the estimation of the belief can be calculated by using the Bayes filter, described in section 2.2. However, due to the fact that the AMCL is used, a particle based filter should be used. Equation 2.42 can not be solved analytically. Due to this fact, the entropy has to be estimated. The entropy of a belief b(x), which is represented by a set of weighted particles can be estimated by [4, 23, 24]: H(b(x)) = d 2 (1 + log(2π)) + 1 log(det(σ)) (2.46) 2

44 Chapter 2. Theory and Background 29 The first term can be ignored for the further calculations, because it is constant. Σ is the covariance matrix of the estimated multivariate Gaussian distribution from the set of weighted particles. The mean of the multivariate Gaussian distribution can be estimated by: µ fit = 1 N η x [i] w [i] (2.47) i=1 N is the count of particles and w [i] is corresponding weight of the particle x [i]. The mean µ fit has to be normalized with η. The normalization constant η is expressed by: η = N w [i] (2.48) i=1 The covariance matrix of the multivariate Gaussian distribution can be estimated by: Σ fit = 1 N η (x [i] µ fit ) (x [i] µ fit ) T w [i] (2.49) i=1 This covariance can be applied to equation 2.46 to determine the entropy of the belief b (x). The estimated multivariate Gaussian distribution is not unbiased. But for a relative high number of particles, representing the belief, this error is minimal and can be ignored. Now, the expected entropy is known. Last but not least, the costs to get to the predicted pose x are required to calculate the trade-off for an exploration action. There are different solutions to calculate the costs on an occupancy grid and are discussed in section 2.6. To calculate the costs r(x, a exp ) an arbitrary selection can be used. Now, all required components to calculate the trade-off are known. The trade-off ρ aexp is calculated as follows for a single exploration action [4]: ρ aexp = r(x, a exp ) α H b (x) (2.50) The coefficient α is a constant value, which transforms the expected entropy into the expected costs [4], to get from x to x. This notation is only valid, based on the assumption that the costs r(x, a exp ) 0. The best exploration action, to minimize the entropy can be determined by [4]: a = arg max p aexp (2.51) a exp The described steps will be done for each particles of the belief b(x) and all possible exploration actions a exp of the pre-defined set.

45 Chapter 2. Theory and Background 30 Criterion for Termination Furthermore, a criterion when the process is done and the pose of the robot is accurately known has to be defined. A good criterion would be defined via the covariance matrix of the pose. The covariance matrix of a 2D pose is defined as: σxx 2 σxy 2 σxθ 2 Σ = σyx 2 σyy 2 σyθ 2 (2.52) σθx 2 σθy 2 σθθ 2 If the current pose estimation of the localization process is equal or better than each component of the main diagonal, the pose is accurate enough and the process can be terminated. In the worst case, the previously defined uncertainties as criterion for termination will be never reached and the localization will fail. In this case, it is useful to terminate the active localization process after a predefined time. Pseudocode Therefore, an algorithm for the active localization can be defined. The pseudocode is given by algorithm 11. The algorithm requires the termination criteria Σ terminate and t terminate as input and returns the pose and its covariance, if the robot is localized. Algorithm 11 Active Localization, based on algorithm of [4, p.574] 1: procedure ActiveLocalization(Σ terminate, t terminate ) 2: Initialize the AMCL with b(x) as uniform distribution 3: while Σ Σ terminate or t < t terminate do 4: for i = 1 to N do N is count of particles in belief b(x) 5: sample x b(x) Draw random particle from current state 6: for all exploration actions a exp do 7: sample x p(x x, a exp ) Predict new pose 8: sample z p(z x ) Predict measurement at new pose 9: (b (x), w t) = BayesFilter(b(x), a exp, z) Predict new state 10: r(x, a exp ) = costs to get from x to x Use arbitrary planner 11: (µ fit, Σ fit ) = FitGaussian(b (x), w t) 12: H b (x ) = 1 2 log(det(σ fit)) 13: ρ aexp = ρ aexp + r(x, a exp ) α H b (x ) 14: end for 15: end for 16: a = arg max aexp p aexp 17: Move robot with selected action a With collision avoidance 18: Predict and update b(x), Σ using AMCL Executed at parallel process 19: end while 20: return (x, Σ) 21: end procedure

46 Chapter 2. Theory and Background Unknown Environment - Active Exploration Active exploration addresses the problem of automated model (i.e. map of the environment) building, which is an important task in mobile robotics [10]. The robot is set into a unknown environment and have to measure its environment and decide, where to move next, to gather the most information as possible. To solve the problem, a strategy to explore an unknown environment is required. A number of potential strategies are described in the upcoming sections Frontier-based Exploration Strategy The frontier-based exploration is a simple strategy to explore an unknown environment. It was introduced by Brian Yamauchi in 1997, with the central idea of: To gain the most new information about the world, move to the boundary between open space and uncharted territory [20, 25]. The strategy is based on the current occupancy grid map m t, where all cells of the map are classified as open, occupied or unexplored [20, 25, 26]. Using the classified cells, all frontier cells can be determined. A frontier cell is a cell, which is classified as open and is adjacent (for a 8-neighborhood) to a cell which is classified as unexplored. After that, the strategy groups all frontier cells into frontier regions [20, 25, 26]. A frontier region is a region with adjacent frontier cells. The search of the different frontier regions can be done by different strategies. One possible strategy is to pick up one frontier cell, label it as part of a frontier region and check all of its neighbors (given by a 8-neighborhood). All neighbors, which are also frontier cells will be also labeled as part of the frontier region and their neighbors, too. If no neighbor is a frontier cell, the complete region is determined. After that, a new frontier cell will be picked up and so on, as long as all frontier cells are labeled. In the end, all frontier regions are extracted. The pseudocode for the extraction is given in algorithm 12. The algorithm requires a list of all frontier cells in the current map m t. The algorithm returns the extracted frontier regions. All determined frontier regions will be checked on their size. If a frontier region is less then the size of the robot or an other minimum size [20, 25, 26], such regions will be ignored in the following calculation process. For the rest of the regions, the centroids will be calculated. The centroid of 2D point cloud can be determined via: c x = N 1 region x i,region (2.53) N region i=1

47 Chapter 2. Theory and Background 32 Algorithm 12 Extraction of frontier regions on 2d occupancy grid, based on algorithm 3.2, purposed in [27] 1: procedure 2DGridFrontierRegionExtraction(f cells ) 2: inspect = Saves to inspect frontier cells 3: labeled = Contains all frontier cells, which already part of one region 4: frontier regions = Saves a list of cells for every frontier region 5: while f cells not empty do 6: region = Saves all cells of the frontier region 7: inspect = inspect + front element of f cells 8: while inspect not empty do 9: cell = front element of inspect 10: inspect = inspect cell 11: region = region + cell 12: labled = labled + cell 13: for all neighbors ngh of cell do 14: if ngh is frontier cell and not labled then 15: inspect = inspect + ngh 16: end if 17: end for 18: end while 19: frontier regions = frontier regions + region 20: end while 21: return frontier regions 22: end procedure N 1 region c y = y i,region (2.54) N region N region is the count of cells of the frontier region. Figure 2.9 illustrates frontier cells and frontier regions, where the size of robot is only one cell. As can be seen, the frontier cell in lower left of the picture is not a frontier region, because it contains only of one cell, which is not bigger then the robot itself. i=1 (a) Frontier cells (red colored cells) (b) Segmented frontier regions with centroids (yellow cross with circle) Figure 2.9: Illustration of frontier cells and frontier regions.

48 Chapter 2. Theory and Background 33 The frontier region, which is the next best for exploration, is the region with the minimal distance from its centroid to the current pose of the robot. Due to this, the strategy is also called nearest frontier detection. Criterion for Termination A good criterion for terminating the frontier based exploration process is, if no further frontier region exists in the current map m t any more. Pseudocode The algorithm requires the current map m t and the current pose x t = (x, y, θ) of the robot in the map. Algorithm 13 Frontier based exploration 1: procedure FrontierBasedExploration(m t, x t ) 2: f = List of frontier cells 3: for all cells do 4: if cell is frontier cell then 5: f = f + cell 6: end if 7: end for 8: r = 2DGRIDFRONTIERREGIONEXTRACTION(f) 9: for every region r do 10: if size(region) < size min then 11: r = r region 12: else 13: c regionx = 1 N region N region i=1 x i,region 14: c regiony = 1 N region N region i=1 y i,region 15: end if 16: end for 17: return min x r,y r { (x x r ) 2 + (y y r ) 2 x r, y r centroid of frontier regions} 18: end procedure The algorithm returns the nearest frontier region as next best point for the exploration. The desired heading θ d for the frontier region is the mean of the heading with respect to the centroid [28]. θ d = 1 N N atan2(c regiony y i,region, c regionx x i,region ) (2.55) i=1

49 Chapter 2. Theory and Background Information Gain-based Greedy Exploration Strategy As like for the active localization, a greedy exploration strategy for learning model of the environment can be defined, where the best exploration action is determined by the trade-off between the information gain and the costs. Essentially, it is the same strategy as like for the active localization, but the information gain will be determined in a different way, because no laser scan can be simulated. Important is, that the information gain is treated independent between the different grid cells of the map [4]. The greedy exploration algorithm determine the next exploration action to the position, where the information gain is maximal [4]. In literature [4], three different approaches are shown, which calculates the gain per grid cell: Entropy: The entropy of a cell can be determined as follows [4]: H p (m i ) = p(m i ) log(p(m i )) (1 p(m i )) log(1 p(m i )) (2.56) p(m i ) is the occupancy grid probability of the cell. Using this entropy, cells with a high one indicates that a cell is unexplored. Expected information gain: The entropy before does not take the information of the sensor into account [4]. The expected entropy after sensing is given by [4] [4]: E[H p (m i )] = p + H + p (m i ) + p H p (m i ) (2.57) where H + p (m i ) is the entropy of the posterior that cell is measured as occupied and H p (m i ) is the entropy of the posterior that a cell is measured as free. The probability that a cell will be measured as occupied is given by [4] [4]: p [+] = p true p(m i ) + (1 p true ) (1 p(m i )) (2.58) where p true is the probability that the occupancy is correctly measured and the update of the occupancy grid yield to the following probability [4]: p i = p true p i p true p i + (1 p true ) (1 p(m i )) (2.59) The entropy of the posterior H + p (m i ) follows by using equation Analogous to the probability that a cell is measured as occupied, the probability that cell is measured as free can be calculated with the complementary probability of p [+] and is denoted by p [ ]. Using equation 2.56 again yield to the entropy of the posterior that a cell is measured as free H p (m i ). At the end, the expected information gain

50 Chapter 2. Theory and Background 35 is [4]: I(m i ) = H p (m i ) E[H p (m i )] (2.60) But in fact, visualizing the entropy from before and the expected information gain, the values differs, but they are indistinguishable and using the much simpler calculation of the entropy leads to the same result [4]. Binary gain: This approach assumes that a cell is explored or unexplored and therefore, the expected information gain become binary [4]. It is core of the before discussed frontier based exploration strategy [4]. In contrast to the active localization, the exploration action is the motion to a x-y position following a minimum costs path [4] and it follows that every position in the map is a possible exploration target (greedy approach). Criterion for Termination A criteria for termination of the algorithm is, if no new information can be gathered by moving the robot, i.e. if the expected information gain for every cell is zero. Pseudocode Algorithm 14 Information gain-based greedy exploration 1: procedure GreedyBasedExploration(m t, x t ) 2: for i = 1 to M do M is the count of cells of m t 3: Calculate the expected information gain I(m i ) 4: r(m tcurrent, m ti ) = costs to get from m tcurrent to m ti Use arbitrary planner 5: π(m ti ) = r(m tcurrent, m ti ) α I(m ti ) 6: end for 7: a = arg max mti π(m ti ) 8: return a Return next best exploration action 9: end procedure Viewpoint-based Exploration Strategy The following exploration strategy has been introduced by González-Banos and Latombe. The scope of this algorithm is to build up the map efficiently by determining the best location for exploration, in compliance to quality and amount of the new information [10]. This strategy is similar to a Next-Best View (NBV) problem like in computer

51 Chapter 2. Theory and Background 36 vision and graphics [10]. The key insight of this algorithm is the use of safe regions. A safe region is the largest region in the map, which is guaranteed safe given of all sensor data [10]. The mathematical notion of a safe region is given in [10]. The robot is at a position x k and scans its environment. From these scans, the local safe regions have to be determined. For that all polygonal lines in the current scan are calculated. These can be extracted by clustering all scan points and fitting a line through the clusters [10]. From the extracted set of polygonal lines, the safe region S xk can be determined and is bounded by the polygonal lines and free curves, connecting the endpoints of the lines [10]. Further, the safe region is bounded to the maximal scan range of the sensor. An example for polygonal lines and safe regions is given in figure Figure 2.10: Example for polygonal lines, free curves and safe region for the viewpoint-based exploration, based on [10]. The blue are the determined safe region S(x k ) Now different poses (called candidates) for exploration will be generated. These candidates are uniformly distributed over the computed safe region S xk before and denoted by L candidates. After the candidates are generated, they have to be evaluated. The score for a candidate pose q L candidates can be calculated as follows [10]: g(q) = A(q) e λ l(x k,q) (2.61) l(x k, q) is the length of the collision free path from the current pose to the candidate and λ is constant value, which is used to weight the costs against the information gain [10]. A(q) measures the information gain at candidate pose q. A possible way for measuring the information gain is to simulate a scan at the candidate pose q and count the length of all beams outside the explored space, i.e. the length of all beams up from the intersection of the beam and a free curve. The candidate with the maximum score g(q) is the next target for exploration.

52 Chapter 2. Theory and Background 37 Criterion for Termination In literature [10], the criterion for termination is expressed as follows: If the boundary contains no free curve any more, then the environment is completely mapped. But this test is not used in practice. Instead, the remaining free curves of the boundary will be checked on their size. If the size is lower then a predefine threshold, the mapping has been done [10]. Pseudocode Algorithm 15 Viewpoint based exploration, based on [10] 1: procedure ViewpointBasedExploration(m t, x t ) 2: Compute save region S(x k ) 3: Generate L candidates with N candidates q p(s(x k )) 4: for i = 1 to N do 5: Generate scan at q i 6: Calculate A(q i ) 7: l(x k, q i ) = costs from current pose x k to q i Use arbitrary planner 8: g(q i ) = A(q i ) e λ l(x k,q i ) 9: end for 10: a = arg max qi g(q i ) 11: return 12: end procedure 2.8 Change Detection In general, change detection detects changes in one scene at two different times. The change detection is commonly used in remote sensing, where satellite images are checked on differences. But also for robot based surveillance, change detection is important [29, 30]. In context of robotics, change detection is to determine the difference within the environment, if the robot passes it at another time again. Different methods exists to realize algorithms for change detection. Figure 2.11 summarizes possible methods for change detection.

53 Chapter 2. Theory and Background 38 Figure 2.11: Overview of possible methods to implement algorithms for change detection [31]. Classification-based - 2D Multi-class Occupancy Grid Maps The following method is a classification based method. At first, the 2D mutli-class occupancy grid map (MOG map) has to be introduced. A 2D MOG map is 2D occupancy grid (see section 2.3) that includes semantic information and can be applied to change detection [29]. The classical occupancy grid map has only two states, occupied and free. In contrast, the 2D MOG map can consists of N c states [29]: X = x 1, x 2,..., x Nc Additionally, all grids of the 2D MOG map are always occupied and 2D MOG maps follows probability properties, whereby different theorems like the Bayes filter or KL (Kullback-Leibler) divergence can be adopted [29]. Figure 2.12 shows an example for a 2D MOG map with three different classes: tree, road and building. Figure 2.12: Example of a 2D multi-class occupancy grid map with three classes [29]. Based on the assumption that the points of the 2D grid are uniformly scanned in the direction of the z-axis, the posteriori belief of the Bayes filter for the k-th class is given

54 Chapter 2. Theory and Background 39 by [29]: P (z t x k ) = N k N cells (2.62) N k is the count of grid cells for k-th class, N cells is count of all grid cells in the map and it have to be applied that [29] N c k=1 P (z t x k ) = 1 (2.63) The measurement z t will be interpreted as the result of a mapping campaign. The KL-divergence can be used to define a criteria for change detection. KL-divergence calculates the difference between two distributions. The criteria is defined as follows [29]: N c D KL (P Q) = P (i) ln( P (i) Q(i) ) (2.64) i=1 where P and Q discrete random variables. P can be interpreted as the MOG map of mapping campaign t 1 and Q as the MOG map of campaign t 0. If D KL = 0 the both 2D MOG maps are equally the same. For the case that the two MOG maps differ, D KL is unequals zero and will be increased, the more the maps differ from each other [29].

55 Chapter 3 Sensors and Software The following section gives an overview of the given hardware, the sensors and the software libraries, which have been used to implement the algorithms for the underlying problems. The coordinate systems of the different sensors and the coordinate system of the robot will be described, too. 3.1 Volksbot RT3 For this thesis, the Volksbot RT3 of the Frauenhofer IAIS will be used. The Volksbot RT3 is a robot with two actuated wheels and one castor-wheel. The motors for the two actuated wheels can be controlled via a C++ software library [32]. Figure 3.1 shows different views of the Volksbot RT3. (a) From above [32] (b) From side [32] (c) From behind [32] Figure 3.1: Different views of the Volksbot RT3 from Frauenhofer IAIS Looking from behind onto the control panel of the robot, the mapping of the motors is as follows: The left motor is the motor on the left hand side and the right motor is the motor on the right hand side (see: figure 3.1(c)). 40

56 Chapter 3. Sensors and Software 41 The dimensions of the Volksbot RT3 are shown in figure 3.1 and summarized in table 3.1. Dimension of the Robot Value Unit Lenght 580 mm Width 520 mm Height 315 mm Wheel seperation 425 mm Table 3.1: Summary of important dimensions of the robot 3.2 Sensors Odometry The Volksbot RT3 uses a differential odometry to determine the pose of the robot. The odometry provides how many ticks both motors have done during a time interval t. The count of the motor ticks can be determined by reading the current encoder value for every motor. Using the noise free motion model, described in section 2.1, the pose can be calculated. Uncertainties The accuracy of the calculated pose is effected by different systematic and non-systematic errors [33]. Systematic errors are uncertainties of the parameters in the equation for pose calculation [33]. An example for uncertainties of the the parameters are different pressure of the wheels. Whereas, uneven ground is an example for non-systematic errors [33]. In fact, there are many more systematic and non-systematic effects. The uncertainties as to the position and the orientation, given by the data of the odometry, increase by and by. Figure 3.2 illustrates the increasing uncertainties. (a) Uncertainties in position (b) Uncertainties in heading Figure 3.2: Increase of uncertainties of position and heading by time

57 Chapter 3. Sensors and Software 42 A possible way to determine the uncertainties and correction factors of the differential odometry is the bi-directional square path experiment [34]. In this experiment, the robot drives four meters, turns 90 and repeat this three times more. Figure 3.3 illustrates the test. (a) Square path experiment for the clockwise case terclockwise case (b) Square path experiment for the coun- Figure 3.3: Bi-directional square path experiment, as provided in [34] Determination of c mm The factor c mm has been introduced by the noise free motion model from section 2.1. This parameter has to be empirically determined. The robot will be moved 50cm forward and the ticks of the motors will be logged. This will be performed for 15 times. Resulting from these experiments, c mm = The data for determination can be found in appendix E Laser Scanner Sick LMS100 For the perception of the environment, the laser scanner LMS100 of the company Sick will be used. Every measurement generates a set of range values, where every range value corresponds to a single laser beam. Figure 3.4 visualizes one possible measurement of a scan line. The range from the scanner to an object can be calculated via the following measurement principles: time of flight, phase shift or triangulation [3]. The maximal measurement range in depth of the Sick LMS100 is 20 meters and the angular measurement width is 270 [35]. The measurement of the range by the laser scanner is erroneous. The overall error for the range measurement is given by the

58 Chapter 3. Sensors and Software 43 statistical error and the systematic error. The statistical error is 20 millimeters and the systematic error is ±40 millimeters [35]. Therefore, the overall error is 60 millimeters. Figure 3.4: Visualization of a measurement from a laser scanner 3.3 Coordinate Systems This section describes the different coordinate systems of the used robot platform Volksbot RT3 and its sensors. The transformation from a sensor coordinate system to the robot coordinate system will be described, if necessary Coordinate System of the Robot The coordinate system of the robot is centered in the middle of the axis between the two wheels. It is a left orientated coordinate system, where the x-axis is along the moving direction of the robot, the y-axis is perpendicular to the x-axis and positive to the right hand side. The z-axis complements the coordinate system. Figure 3.5 shows the described coordinate system of the robot platform. (a) Source for picture of robot: [32] (b) Source for picture of robot: [32] Figure 3.5: Coordinate system of the robot

59 Chapter 3. Sensors and Software Coordinate Systems of the Sensors Odometry The odometry coordinate system equals the coordinate system of the robot. (a) Source for picture of robot: [32] (b) Source for picture of robot: [32] Figure 3.6: Coordinate system of the odometry There is no transformation between the odometry and the robot coordinate system required, because both coordinate systems are centered in the middle of the robot and left orientated systems (compare: figure 3.5 and figure 3.6). Laser scanner The origin of the laser scanner s coordinate system is centered in the middle of the scanner. It is a 3D left orientated system. The x-axis is along the moving direction of the robot and the y-axis is positive at the side to the motor on the right hand side and complements the left orientated coordinate system. Figure 3.7: Coordinate system of the laser scanner. Source for the figure of the LMS100: [35]

60 Chapter 3. Sensors and Software 45 The scanner s coordinate system has a displacement to the robot s coordinate system. It is a translation in the x-direction of 125 millimeters. To transform the Cartesian coordinate of a scan point to the robots coordinate system, the following transformation has to be calculated: x Robot y Robot = x LMS y LMS z Robot z LMS (3.1) 0 No rotation has to be applied, because both coordinate systems are left orientated systems and the axises have same orientation in space. The units in equation 3.1 are meters. 3.4 Software ROS - Robot Operating System To implement and test the solutions for the functions and tasks, the Robot Operation System (ROS) will be used. ROS is a free and open-source operating system for robots [36]. ROS is a collection of software frameworks for robot software development. It is a Linux based system which provides a communication layer to the robot s hardware [36]. ROS provides different libraries and tools to implement own algorithms and applications for robots. Currently different versions of ROS exist. The newest released is Hydro, but Groovy will be used for this thesis. Different programming languages can be used to implement the new applications and algorithms. The languages, which can be used are C++, Python, Octave and Lisp [36]. The different libraries are available for all of these four programming languages. The ROS implementation consists of nodes, messages, topics and services [36]. A node is a process that executes algorithms and computations. An application can have many different nodes, working simultaneously. Single nodes are able to communicate and transfer informations via so called messages. A message always consists of a header and multiple fields, which define the content of the message. ROS implements predefined messages for different contexts, for example: Standard messages: This package of message types contains wrapper for all primitive types of ROS and the type Empty. The type Empty can be used to send a signal to another node, e.g. for notification [37]. For example, this will be used to initialize the AMCL for global localization. Sensor messages: This package contains messages to communicate sensor related information. There are predefined messages for a laser scanner, the odometry, an

61 Chapter 3. Sensors and Software 46 Inertial Measurement Unit (IMU) or Global Positioning System (GPS) and many more. Geometry messages: These messages describe geometric objects like points in 2D- or 3D-space, poses with or without uncertainties and further types. Navigation messages: This package provides messages to communicate a map, the robot s pose related information and other information for navigation tasks. There are also messages defined for visualization and other tasks (see: ROS Wiki [37]). Additionally to these predefined messages, own messages can be created. The fields of an own defined message can be other message types and primitives types as well. A node can broadcast a message by publishing this message to a topic, which is only a string. Nodes, which are interested in the data of the topic will subscribe to this. A node can publish and subscribe to different topics at the same time [36]. If an information is only required for a specific time, services are very useful. A service is defined by a string, a response and a request [36]. The service can be triggered via the request and returns its answer via the response, which are both messages. Tools Furthermore, ROS has many tools for visualization, like sensor data, using RVIZ or the structure of a ROS application (nodes and topics), using rqt graph. Nodes are visualized as ellipses and topcis as rectangles with their names inside. The node is connected via a line with the topic. An example for the visualization of the sensor data with the tool RVIZ is shown in figure 3.8. It shows a possible way to visualize the particles of a particle filter on a map. The particles are visualized as arrows with the origin at the position of the corresponding pose and a certain direction, which corresponds to the heading. Figure 3.8: Example for visualization with the tool RVIZ. In this case the particles of a particle filter are shown on the map.

62 Chapter 3. Sensors and Software 47 Figure 3.9 shows a possible result of the tool rqt graph, which visualizes the working nodes and topics. In the example the following can be seen: The nodes ps3 joy, volksbot, cmd vel volksbot, lms100, map server, pubtf and amcl are running. For example, the node ps3 joy communicates with the node cmd vel volksbot via the topic joy. The topic tf is both, subscribed and published by the node amcl. Figure 3.9: Possible output of the tool rqt graph All topics can be logged and playbacked for debugging and simulation with real data, using the tool rosbag. Also, all active topics can be listed and their messages printed on the console, using the command line tools of rostopic. To check out, which other tools exist, see the ROS Wiki [37]. Existing packages for driver and algorithms ROS consists of many packages, which provide different drivers and algorithms for robots. There exist drivers for common sensors in robotics, like laser scanner, motor controller, IMU or GPS. These driver nodes broadcast the sensor related information in own topics. Also, common algorithms in robotics, like SLAM, motion planning for robot arms or autonomous navigation and utilities libraries, like calculating the transformation between different coordinate frames or a particle filter, are available. However, there are many other packages for specific robot platforms and other tasks, too. Learn ROS An overview of all libraries and tools is provided by the ROS Wiki [37]. Furthermore it contains an installation guide and tutorials to learn the basics of ROS. A helpful book to learn ROS is Learning ROS for Robotics Programming [38]. The drawback of the book is the use of ROS Fuerte, an old version of ROS, but the main concepts of ROS are still valid.

63 Chapter 3. Sensors and Software Gazebo For the simulation of the different algorithms of this thesis and maybe further exploration algorithms, Gazebo will be used. Nowadays, Gazebo is a stand alone open source project for robot simulation [39]. Up to ROS Groovy, it was an extension of ROS [40]. Gazebo allows to create 3D models of arbitrary environments and robots and their sensors. Figure 3.10 shows an example of a 3D model of the office of Willow Garage and a 3D model of a robot. (a) Source for picture of 3D model: default model of Gazebo fault model of Gazebo (b) Source for picture of 3D model: de- Figure 3.10: Examples of 3D models using Gazebo To create a 3D model of a robot, the Unified Robot Description Format (URDF) file format can be used. URDF is a XML based description format. In general, the root link for the description is the base of the robot. All leafs of this root are sensors, wheels and other components. Using the XML macro language xarco, the creation of such URDF will be simplified. For example, if all wheels of a robot are equally the same in physical properties and looking, a macro for the wheels can be created and the position of the robot will specify, where the wheel will be set. 3D models of an arbitrary environment can be included via the use of COLLADA files. COLLADA is also a XML based description language, like URDF, but will be used for 3D models of building and other objects. This COLLADA file can be imported into the SDF files of Gazebo, which is also XML based. SDF is the internal format of Gazebo to store the different models and worlds. An advantage of Gazebo is that the topic of the laser scan will be generated from the 3D model and is available by listening on the corresponding topic. By adding a plugin for the odometry, the odometry topic can also be simulated by Gazebo. This makes it easy to use Gazebo with ROS nodes, listening on these sensor topics. Not only the laser scanner and the odometry topics can be simulated using Gazebo, but also further plugins, other sensors can be simulated and published.

64 Chapter 3. Sensors and Software 49 Learn Gazebo A good and brief introduction, using Gazebo for 3D modeling and running it in combination with ROS is given in Learning ROS for Robotics Programming [38]. Also, the project website of Gazebo [39] has many tutorials.

65 Chapter 4 Scenarios, Implementation and Experiments The upcoming chapter describes, how the methods of chapter 2 are implemented and the parameters configured to solve the underlying problems of this thesis. The scenarios and the experiments to evaluate the solutions are described in this section, too. 4.1 Map of Basic Scenario - IKG Floor For the experiments a map of the basic scenario is required. The map was created based on an AutoCAD file, which contains the layout of the Institute of Cartography and Geoinformatics floor at the Leibniz Universität Hannover. Figure 4.1: Line map of the IKG floor used as occupancy grid map in ROS All obsolete information is removed from the layout. At the original file the walls are gray hatched. This gray hatching is replaced by black color. The reason for this is to use the map as an occupancy grid map in ROS. The result of the generated line map is shown in figure

66 Chapter 4. Scenarios, Implementation and Experiments 51 Meta Information of the Map for ROS To use an occupancy grid map in ROS the created picture requires meta information. The following meta information of the map is required: Path of the image: The path of the image (relative or full qualified), where the image with the map is located on the computer. The image should be a gray value picture and the file format has to be.png. Resolution: The resolution of the map is given in meters per pixel. Equations 4.1 to 4.3 describe, how to calculate this resolution. Origin: The origin describes the 2D pose of the lower-left pixel of the map. The angle of the 2D pose describes a counterclockwise rotation of the map. If the angle is zero, there is no rotation applied [37]. Threshold (Occupied): This threshold defines a lower bound for interpreting a pixel as occupied space. If the occupancy probability of a pixel is greater than this lower bound, the pixel represents occupied space. Threshold (Free): This threshold defines an upper bound for interpreting a pixel as free space. If the occupancy probability of a pixel is lower than this upper bound, the pixel represents free space. Negation: If this flag is set, the interpretation of the map is reserved, but the thresholds, defined above, are not effected [37]. A negation means that free space will be interpreted as occupied space and vice versa. The occupancy probability is described by the gray-value of the pixel. The darker a pixel, the higher is the occupancy probability. For a white pixel, the occupancy probability is zero and and for a black pixel it is one. To calculate the resolution of a map, the scale between the picture and the real world and the spatial printing dot density of the picture are required. The scale will be denoted as s wm and additionally the spatial dot density will be denoted as dpi. The unit of dpi is inch. At first, the dpi has to be converted to millimeters: dpi mm = dpi 25.4 (4.1) Dividing dpi mm with the scale, the inverse of the resolution follows: r inv = dpi mm s wm (4.2)

67 Chapter 4. Scenarios, Implementation and Experiments 52 Inverting r inv and scaling to meters per pixel, the correct resolution r can be calculated. r = 10 3 r inv (4.3) 4.2 ROS-Driver for Odometry To handle the differential odometry of the Volksbot RT3, the volksbot uos driver 1 is used. This driver defines a right orientated coordinate system. In section 3.3.2, the coordinate system of the odoemtry is defined as left orientated. Therefore, the driver is adapted to the left orientated coordinate system. 4.3 Autonomous Navigation with Collision Avoidance For navigating the robot autonomously with collision avoidance to a goal, the move base package of ROS will be used. A detailed description is given in appendix B. At first, the footprint of the robot has to be defined. The footprint of the robot describes the principle form and properties of the robot. The origin of the robot is in the center of the wheel axis (see: figure 3.5). From that, the footprint is defined in a counterclockwise sense with the first edge at the left wheel (Cartesian coordinate of left edge are (0.125, )), the second edge at the left end of the robot etc. The last two edges of the footprint are at the right and left front edges of the laser scanner. Figure 4.2 shows the footprint of the robot. Figure 4.2: The footprint of the robot with its coordinates. During the active localization task, the robot s pose isn t known in the map. Therefore, only local goals can be defined to drive to. By default, the move base will be used to navigate to global goals, i.e. the goal is given in the map coordinate frame. To drive to local goals, the move base has to operate in the odometry coordinate frame of the robot. 1 Available at: driver (branch: master)

68 Chapter 4. Scenarios, Implementation and Experiments 53 For the active exploration task, the move base has to run in conjunction with GMapping. Therefore, the move base has to be configured that it is able to handle non-static maps. The concrete configuration for both cases can be found in appendix B. Due to the fact that the robot perceives its own chassis (see: figure 4.3), the minimum and maximum scan angle have to be restricted. Therefore, the scan angle is limited to [ 90 ; +90 ]. This will be done by filtering the scan topic using the filtering package of ROS. If the laser scanner data is not filtered the robot is unable to turn in place, because the move base detects a collision, effected by the perception of its own chassis. (a) Robot scans own chassis without fil- (b) Robot does not scan its own chassis, tering the laser scanner data. Scanner by filtering the laser scanner data. robot (red). data (blue) is within the footprint of the scanner data (orange) is within the footprint (red). Figure 4.3: Visualization of perception of the chassis with/without filtering the laser scanner data No 4.4 Exploration The following section describes the implementation aspects for the localization and exploration tasks, the parameter configurations for the existing and used ROS nodes, the simulation with Gazebo and further aspects Active Localization Adaptive Monte Carlo Localization for Active Localization For the global localization task of the active localization, the AMCL will be used and is already implemented in the navigation stack of ROS. To use this implementation, some transformations between frames and some parameters have to be set.

69 Chapter 4. Scenarios, Implementation and Experiments 54 At first, the frames for the map, odometry, robot and the laser range scanner have to be defined. ROS defines a hierarchy for the different frames. The map frame and the odometry frame define a world fixed coordinate system, where the odometry frame is subordinated to the map frame. The robot frame is attached to the center of the robot and is subordinated to the odometry frame. The frame of the laser range scanner is subordinated to the robot frame. With all of these frames, the complete transformation tree is well defined. This is very important, otherwise the AMCL can not be performed. The transformation tree is shown in figure 4.4. Figure 4.4: The transformation tree for using AMCL node. To communicate the transformation, the different sensor nodes have to publish the transformations. Therefore, the odometry node has to publish the transformation between the frame of the odometry and the robot. This transformation is defined by the current absolute pose of the odometry at the time t. An additional node publishes the constant transformation between the laser range scanner frame and the frame of the robot, every time a transformation between the odometry and the robot frame is published. This transformation is defined in equation 3.1. The result of the AMCL is the approximated transformation between the map frame and the frame of the robot. With the resulting transformation of the AMCL, the transformation between the odometry frame and the map frame is also defined. Important is, if a transformation from one to any other frame is given, the inverse transformation is also known by ROS. To start the AMCL on the Volksbot RT3 some parameters have to be adapted. Which parameters are to set and how these set is described in appendix A. As like as for the move base, only the filtered laser scan will be used, to avoid short readings regarding to the perception of the own chassis Generating Initial State for Active Localization At the start of the active localization, the pose of the robot is completely unknown for the global localization problem. The AMCL will be initialize the particles by drawing

70 Chapter 4. Scenarios, Implementation and Experiments 55 valid poses (and therefore, valid particle for the initial state) randomly from an uniform distribution, which covers the whole map. A pose is only valid, if it is located at a cell, which represents free space (see: section 2.7.1). Figure 4.5 shows an example for valid and invalid poses. (a) Valid poses (b) Invalid poses Figure 4.5: Examples for valid and invalid poses in context of an occupancy grid map Implementation of Active Localization Algorithm Perform Random Movement As can be seen, the computational costs are very high to determine ρ aexp for all particles of b(x) and all exploration actions a exp. One simple and suggested option to reduce the computational costs, is to perform random movements in the beginning, until the particles are concentrated on some local maximums and not uniformly distributed [21]. When generating random movements, the effect of the Brownian motion has to be avoided. The expected value of the Brownian motion is zero with a given variance. Therefore, it is useful to uncouple the translational and the rotational parts of the movement and to select the random translation big enough. Both will be drawn from an uniform distribution: 1 s s p(s) = 2 s 1, s 1 s s 2 (4.4) 0, otherwise 1 θ θ p(θ) = 2 θ 1, θ 1 θ θ 2 0, otherwise (4.5)

71 Chapter 4. Scenarios, Implementation and Experiments 56 For the translational part of the movement, s 1 = 1m and s 2 = 3m are chosen. For the rotational part of the movement, θ 1 = π and θ 2 = π are the bounds. Initialization of the Exploration Actions The exploration actions for the search can be precomputed, because they are targets relative to the robot. The value for the size of the search grid cells can be set higher than the value for the size of the cells of the occupancy grid map. This can be done, because due to the robot s uncertainties moving in the real world the navigation error is bigger than the size of the cells of the used occupancy grid map. For the implementation the cell size of the search grid is set to 1.0 meters. The width and the height of the search grid are 10.0 meters. A constraint for the selection of the width and the height of the search grid is the size of the local cost map for executing the move base. In particular, this means that the size of the local cost map determines the maximal height and width for the search grid. The search grid will have to be smaller than the size of the cost map. If this is not considered, it can happens that the robot is not able to get to the goal. A work around idea for this case is to split the movement into movements, which are smaller then the local cost map of the move base and perform them sequentially. Draw random particle In line 5 of the pseudocode for the active localization, random particles from the current state b(x) have to be drawn. The particles will be drawn from uniform distribution: 1 N x b(x) =, 0 n N (4.6) 0, otherwise Once a particle was drawn, it can not be drawn again, i. e. it will be drawn from the distribution without replacement. Predict new pose depending on exploration action a exp The predicted pose, depending on the current exploration action a exp has to be sampled from the distribution x p(x x, a exp ). In this implementation, a perfect motion will

72 Chapter 4. Scenarios, Implementation and Experiments 57 be assumed in this step. Therefore, the calculation of the new pose x is given by: x x y x θ x x x + x 2 a exp + ya 2 exp cos(θ + φ) = y x + x 2 a exp + ya 2 exp sin(θ + φ) θ + φ With the bearing angle φ = atan2(y aexp, x aexp ) from x to x. (4.7) Omitting new poses Due to the fact that the search grid is around a randomly drawn pose, it can happens that a new predicted pose x is outside the range of the map, in an unexplored area or in occupied space. If one of these cases happen, x will be ignored and a fixed penalty of a negative value will be added to p aexp for the exploration action a exp. Prediction of measurement at pose x To predict the measurement z at the predicted pose x, the distance from x to the next obstacle in the map m has to be calculated. This will be done via ray casting. Ray casting is a possible way, to get the next object in the real world, using a given map and the current pose. The ray casting will return a distance zray k for the k-th beam. Like the prediction of the new pose, no noise has been considered so far. It will be taken into account represented by a Gaussian distribution. Therefore, the calculation of a single measurement z k is given by: z k p(z k x ) = N (z k ray, σ 2 noise,z) (4.8) The standard deviation is the sum of the statistical error and the systematic error of the LMS100 (see: section 3.2.2). Therefore, σ noise,z = This value is true for all k beams of the scan, which a measurement contains. Due to the fact that not all beams are used to proceed the update step of the particle filter, as mentioned before when the AMCL parameter max beams, the laser scan will only be partially simulated. Therefore, only z k for the k-th beam will be calculated. The step size is given by the quotient of the count of rays per scan and the maximum count of beams to check. Implementation of the Bayes filter for state estimation To estimate the new believe b, a particle filter is the best way for implementation,

73 Chapter 4. Scenarios, Implementation and Experiments 58 because b(x) is represented by a set of particles. The implementation of this particle filter has a fixed count of particles, which depends on the current count of particles of b(x). As motion model, the model from section will be used. It is the same as for the AMCL. The used measurement model is the likelihood field model, which is described in section Both models require some parameters, e.g. p hit, p rand, σ hit, α 1, α 2 and further. These values are determined in appendix A and are used for the implementation. α 4 is set to zero. Fitting the Gaussian Distribution To fit the Gaussian distribution, only the position in x and y is used. The heading will be ignored for simplifying the calculation process, because the weights are also part of the estimation process. If the heading is completely wrong, the weight of particle is very low, due to the fact that the likelihood of the update step is low for the particle. Calculation of the costs r(x, a exp ) To calculate the cost, the breadth first search will be used. If the robot wants to pass an obstacle or unexplored space, this will be penalized. Due to the fact that all costs from the start cell to all other cells will be calculated by the breadth first search, the costs can be determined by a lookup on an array with constant computational time. Additional Reduction of Computational Time An additional speed up is to pre-compute the distances to the occupied spaces in the map, because the ray casting algorithm has high computational costs. The distances are the error free measurements of the laser scan. After that, the determination of the distance from the pose to an object is only a lookup on a data structure with logarithmic cost with respect to the count of free cells in the map. For saving the distances of a cell and for a specific bearing angle, a map with integers and vectors will be used. For the calculation of all distances, every cell of the map is discretized into a set of different bearing angles with an increment of 0.5 degrees. During the discretization process there will be an error, which can be ignored. Unexplored or occupied space will be ignored.

74 Chapter 4. Scenarios, Implementation and Experiments Active Exploration Simulation with Gazebo To create a simulation for the active exploration, the following models and nodes have to be developed: 3D-Model of basic scenario: To create the 3D model of the basis scenario, the free version of the Sketchup is used. Sketchup is a software to create 3D models of everything you want. Figure 4.6 shows the 3D model, which was created with sketchup. Figure 4.6: 3D model of the basic scenario, created with Sketchup This 3D model can be exported as COLLADA file. This file format is supported by Gazebo (see: section 3.4.2). The exported 3D model will be inserted into the Gazebo environment and the 3D model is available as standard model. As can be seen in the model, the doors to the toilets are closed, because the rooms are very small and the robot can not move into the rooms with only one observation source in front. 3D-Model of VolksbotRT3: As like as for the 3D model of the IKG, a model for the Volksbot RT3 and its wheels were created. The two active wheels in front are added separately, otherwise the odometry data can not be simulated. The passive wheel is also not part of the base and is additionally modeled. The 3D model of the base is shown in figure 4.7.

75 Chapter 4. Scenarios, Implementation and Experiments 60 Figure 4.7: 3D model of the base of the Volksbot RT3 3D-Model of wheels: As mentioned before, the two actuated wheels have to be modeled separately. A Sketchup created model causes errors, like the robot rotating randomly or flying around. Using the cylinder model of Gazebo resolves these problems. For the wheels the cylinder has a length of 85 millimeters and a radius of 130 millimeters. The passive wheel is also created with the cylinder model of Gazebo with a length of 60 millimeters and a radius of 100 millimeters. 3D-Model of LMS100: For the laser scanner, an additional 3D model is required. The hratc2014 framework 2 package contains a 3D model of the Sick LMS100 laser scanner. This model will be used for the simulation and is shown in the upcoming figure. Figure 4.8: Detailed 3D model of the Sick LMS100 Create robot model for Gazebo: Now, all single models (robot body, actuated/passive wheels and sensors) have to be merged to create in one robot model. Therefore, the URDF format is used. At first, the link for the base footprint will be defined. This link is defined as an imaginary link to avoid problems with Gazebo. Next, the base link will be defined. The base link contains the model for visualization and collision properties of the base and defines the physical entities (moment of inertia, size and mass). The moment of inertia for the base of the 2 Package available at: framework (branch: master, accessed: )

76 Chapter 4. Scenarios, Implementation and Experiments 61 robot, with length l, width w and height h, can be calculated as follows [41]: I base = 1 12 m(h2 + l 2 ) m(w2 + l 2 ) m(w2 + h 2 ) (4.9) The base footprint and the base link are connected by a joint. Now, the actuated wheels are added to the model. Every wheel has an own link. The moment of inertia for the wheels, with radius r and width w, is defined as [41]: I wheel = 1 12 m(3r2 + w 2 ) m(3r2 + w 2 ) mr2 (4.10) The links for the wheels are connected via a joint with the base link. In the end, the Sick LMS100 has to be added. As before, a link will be defined with the moment of inertia, given in equation 4.9 and will be connected with the base link via a joint. Interactive simulation using odometry data: Currently, the robot model is static. Now, the odometry data have to be simulated. Due to the simulation, no data of the motor ticks are given and the noise free model from section 2.1 can not be used. An alternative is given in the erratic robot 3 package, which calculates the pose as follows: x t y t θ t d left = dt d wheel 2 v left (4.11) d right = dt d wheel v right (4.12) 2 d x left +d right t 1 = y t cos(θ t 1 ) d left +d right 2 sin(θ t 1 ) (4.13) θ t 1 d left d right w In the equations, d left and d right are the driven distance of the wheels during one update decade. v left and v right are the current velocities of the wheels and dt is the decade between two updates. w is the wheel separation. This information will be published, each time a new velocity command will be received from the move base or an other device, publishing velocity commands. Now, the changes have to be shown in Gazebo, by updating its environment. The erattic robot package contains a plugin for a differential odometry. This plugin can be used with one modification. The name of the odometry frame has to be 3 Package available at: robot/tree/groovy (branch: groovy, accessed: )

77 Chapter 4. Scenarios, Implementation and Experiments 62 changed to odom combined (defined by odometry driver of the real robot). Now, the plugin has to be added to the model. There are some properties, like wheel diameter, axes length and further, which have to be set for a correct simulation. Interactive simulation using laser scanner data: The physical engine of Gazebo generates, depending of the environment, the range data of the laser scanner as a topic, which can be filtered and used as usual. Therefore, the link of the laser gets a plugin. The plugin has to be configured, to simulate the correct properties. Add uncertainties to the simulation: To model uncertainties for the odometry, the physical properties of the engine can be changed. The values for µ 1 and µ 2 will be set to 100. Also, the noise for the laser scanner can be modeled by using the plugin of the Gazebo library. For the scanner, a Gaussian noise with standard deviation of 0.06 meters is set SLAM for Active Exploration The Rao-Blackwellized Particle Filter Occupancy Grid SLAM, described in section 2.5, is called GMapping in ROS. As like before, running the AMCL, all transformations have to be published that the transformation tree from figure 4.4 applies as well. Figure 4.9: The transformation tree for using GMapping node. Like for the move base and the AMCL, the filtered laser scan data will be used that the laser scanner does not perceive the chassis of the robot. The upcoming listing shows all parameters, which have to be changed to run the GMapping node with the Volksbot RT3: maxurange: This parameter sets the maximum scan range of the used laser scanner. The maximum scan range is set to 20 meters, which is the maximum scan range of the Sick LMS100 [35].

78 Chapter 4. Scenarios, Implementation and Experiments 63 delta: Defines the resolution of the map in meters per pixel. This parameter is set to 0.05 meters per pixel, because the overall error is 0.6 meters of the laser scanner and to handle uncertainties, which are not modeled in labor. maxrange: If this parameter is set and no obstacle is perceived, all space within the maximum range is set as free in the map. This can lead to additional uncertainties. Therefore, this parameter is not set Implementation of the Exploration Strategy The upcoming section gives some hints on the implementation process for the discussed frontier based exploration strategy in section Due to uncertainties and errors during the mapping process, it can happen that some exploration actions or targets are not executable, because the robot can not move through walls or will get stucked, if it moves to the target. For that, every exploration strategy has to handle a list of non accessible targets, which were additionally checked to decide, if the exploration process is completed or not and to choose the next best exploration target. This idea is proposed in, e.g. [20], [26] or [25]. Before running any exploration strategy, the robot performs an in place rotation of 2π, to gather information what is behind. At the end, if no further exploration target is found, the robot drive back to its start position that loop closing is performed. After driving to the start position, it can happens that some new possible targets are available, because parts of the scenario are observed new. This case is shown in figure (a) Explored map before loop closing. Red (b) Explored map after reaching start location marked targets could not be reached, due to the for loop closing. Now new regions for exploration targets are available (orange marked fact that these targets are quiet near to walls. cells). Figure 4.10: Illustration of the case that new targets available after loop closing.

79 Chapter 4. Scenarios, Implementation and Experiments 64 Frontier Detection based Exploration Save list of regions: The best way to save all frontier regions is to use a data structure of the form vector<vector<cell index>>. To save additional information, a single struct for a frontier region was defined. The struct contains all cells in a vector (vector<cell index>), belonging to the frontier region (as proposed before), the centroid of the region and a flag, if the frontier is accessible or not. With the definition of the struct, all frontier regions are saved as vector<frontierregions>. Choosing threshold size min : This threshold is chosen to size min = 2.00 meters, due to the following: The region could be a door, where the robot get stuck, or the region can be near to an edge of walls, where the robot also get stuck. Abort exploration targets near to walls: It can happen that exploration targets are quite near to walls. This causes that the robot will get stucked. To avoid this, an additional node is created, which checks, if an exploration target is quiet near to a wall and cancels the navigation to the target. After this the target is added to the list of non accessible targets and a new exploration target will be determined. For resolution of 0.05 meters per pixels this buffer is set to 11 cells, which means 0.55 meters in the real world. Additional movements: After a frontier region is accessed, the robot performs additionally a turn in place movement of 2π [25, 26]. If the rotation is not possible, due to the fact that the robot is quiet close to a wall, this additional movement will not be done. 4.5 Change Detection For the simple change detection, the classes for the MOG map are the following: occupied, free and unexplored. Therefore, the classification of the cells is adapted to the provided map of the GMapping implementation of ROS and the provided occupancy grid map could be used for change detection.

80 Chapter 4. Scenarios, Implementation and Experiments Experiments Active Localization For evaluating that the algorithm for the active localization works, optimal test cases are created. The upcoming figures show the different scenarios for evaluating the algorithm. In figure 4.11, the particles are put on the two floors with contrariwise headings (visualized with arrows). Particles in same floor have the same heading. The second scenario is shown in figure 4.12, where the particles are set on the same floor, all with the same heading (visualized with an arrow). As third scenario, the particles are put on the two floors and looking onto the side walls of the floors. This scenario is visualized in figure Figure 4.14 shows the fourth test scenario. There, respectively one cluster of particles is set on the both ends of the floor, with different headings. In all scenarios, every cluster contains of 20 particles. Figure 4.11: Visualization of the initial situation of the first test scenario for the active localization. The headings of the clusters are illustrated with the arrows. Figure 4.12: Visualization of the initial situation of the second test scenario for the active localization. The headings of the clusters are illustrated with the arrows.

81 Chapter 4. Scenarios, Implementation and Experiments 66 Figure 4.13: Visualization of the initial situation of the third test scenario for the active localization. The headings of the clusters are illustrated with the arrows. Figure 4.14: Visualization of the initial situation of the third test scenario for the active localization. The headings of the clusters are illustrated with the arrows Active Exploration For evaluating the active exploration, the robot will be spawned on different locations of the basic scenario and the autonomous exploration will be started. The different start locations are illustrated in figure Figure 4.15: Illustration of the different start locations for evaluating the active exploration.

82 Chapter 4. Scenarios, Implementation and Experiments 67 The x- and y-coordinates for spawning (initial position of the robot at the start of the simulation) the robot can be found in appendix C in table C.1. The different experiments are executed in the simulation and with the real robot Change Detection To evaluate the functionality of the change detection a simple experiment will be done. At first, the robot will be spawned in an empty room, which is illustrated in figure After that, the robot maps this room, using an exploration strategy. In the next step a new obstacle will be added to the room. This case is shown in figure The robot maps the room again. As last run for the experiment, the location of the obstacle will be changed and the robot maps again. Figure 4.16: Initial situation for change detection. The robot maps an empty room. Figure 4.17: Second configuration of the room. Now, an obstacle is added to the room.

83 Chapter 4. Scenarios, Implementation and Experiments 68 Figure 4.18: Third configuration of the room. Changed the position of the object.

84 Chapter 5 Results and Evaluation The upcoming chapter shows the results of the different experiments, described in sections 4.6.1, and The different implemented methods will be evaluated on different criteria and additional observations will be given. 5.1 Challenges with AMCL and move base AMCL Using the generated map from figure 4.1, some problems arose for the active localization. The localization process, using the AMCL failed by using the generated map. To overcome these problems, a map of the basic scenario has been created, using GMapping. The map is shown in figure 5.1. Figure 5.1: Created map with GMapping, to overcome the problems, which arose by using a gray value picture of the blue print. 69

85 Chapter 5. Results and Evaluation 70 move base Problems for the autonomous navigation of the robot are due to the point that the environment is not fully observable by the laser range scanner. The scanner is mounted on a fixed height above the floor and only scans a 1D line. Due to this fact, some objects with a minor height can not be observed. This can lead to situations, where the execution of a navigation task has to be stopped immediately, to avoid damaging the robot and its sensors. The autonomous navigation has a problem in small corridors, due to uncertainties in the map or localization. The path planner sometimes fails to turn the robot in that way, that the robot does not get stucked. In fact, it can happens, that the localization or exploration task fails. Further problems are that the move base does not perform turn-in-place operations, or reach its goal, but does not turn to the goal heading and the move base failed to get to the goal. 5.2 Active Localization Evaluation of the Active Localization by Simulated Data In the previous chapter 4 in section 4.6.1, four different scenarios are described. Figure D.2 shows ρ aexp for the first scenario from figure 4.11 and the defined exploration actions. Figure 5.2: Result of the greedy search for the next best exploration action to execute to determine the pose of the robot for scenario shown in figure The more red a cell of the search grid, the more likely it is to choose this exploration action. For this experiment, the exploration action a exp = (5.0m, 5.0m) would be chosen. The chosen exploration action to solve the localization problem is (5.0m, -5.0m), which corresponds to the most red cell in figure D.2. If the chosen exploration action would

86 Chapter 5. Results and Evaluation 71 be applied, the robot would drive to the blue dots in figure 5.3 and the ambiguity of the pose estimation is solved. Figure 5.3: Where the robot would move to, if the chosen exploration action a exp = (5.0m, 5.0m) would be executed. In compliance with the uncertainties, this exploration action will be chosen and not an exploration action, where both clusters will ends up at the same location after executing the exploration action. The result of the active localization for the scenario in figure 4.14 is shown in figure 5.4. The best exploration action to solve the localization is a exp = ( 7.0m, 0.0m). Figure 5.4: Result of the greedy search for the next best exploration action to execute to determine the pose of the robot for scenario shown in figure The more red a cell of the search grid, the more likely it is to choose this exploration action. For this experiment, the exploration action a exp = ( 7.0m, 0.0m) would be chosen.

87 Chapter 5. Results and Evaluation 72 Figure 5.5: Where the robot would move to, if the chosen exploration action a exp = ( 7.0m, 0.0m) would be executed. It can be seen, if the robot is driving to the end of the corridor, the expected entropy is higher, because the distance between the clusters will be higher as before. Therefore, it makes sense that the robot should drive into the middle of the corridor. Furthermore, it would be assumed that it is also a good solution to drive into the big room next to the left particle cluster. Why this solution is not highly rated could be an effect of the resampling in the Bayes filter step of the algorithm and particles of the right cluster are still part of the state and effect a high expected entropy. The results for the other scenarios can be found in appendix D Best Parameter Combination The active localization algorithm has some parameters to set in order to return the best and correct results. The first parameter, which is reviewed here is α, which transforms the expected entropy into the costs. For the determination of α, the experiment from figure 4.11 is used. To minimize the entropy, the robot has to move into the middle corridor, i.e. the expected exploration action to choose is near to a exp = (5.0m, 5.0m). Figure 5.6 shows result for different α s.

88 Chapter 5. Results and Evaluation 73 (a) α = 1 (b) α = 2 (c) α = 3 (d) α = 4 (e) α = 5 (f) α = 6 Figure 5.6: Result of the active localization for different α s. As can be seen from figures 5.6(a) to 5.6(f), a value greater than five transforms the expected entropy into the costs that it yields to the correct results and an exploration action near to a exp = (5.0m, 5.0m) will be chosen. Further, it can be seen that the higher the parameter α is chosen, the longer exploration actions are allowed to drive (exploration action (5.0m, -5.0m) get high weighted by increasing α). For the implementation it has been decided to penalize exploration actions, which end up in unexplored or occupied space. For the expected entropy, the penalty is that the entropy is the same as before, i.e. the information gain is zero. To penalize the costs is more difficult. Different experiments with fixed amounts of costs like -5, -10 or -20 lead to wrong results. The best penalty for the costs is the following: Due to the fact that it is unknown what is at the unexplored space, the costs for the exploration action are the Euclidean distance. Further, an additional penalty will be added, because it is assumed that the robot has to pass a wall, to get into the unexplored space. This additional

89 Chapter 5. Results and Evaluation 74 penalty is given by p costswall and is set to 10. Table 5.1 summarizes all parameters and how these parameters will be set that the active localization algorithm works well. Parameter Description of Parameter Value Transformation parameter, to α translate the expected entropy > 5 into the costs Penalty for entropy, if an applied exploration action would move the robot H (x) occupied,unknown H into occupied or unexplored (x) = H(x) space Penalty for costs, if an applied exploration action would move the robot a r(x, a exp ) expx + a 2 expy occupied,unknown into occupied or unexplored +p costswall space Table 5.1: Summary of good working combination of values for the different parameters of the active localization algorithm Active Localization by Means of a Real Robot When applying the algorithm for the active localization to the Volksbot RT3 as proposed in section , at first random movements will be performed. This works, but the particles are distributed across too many single clusters. If the random movements are performed too often, it can happen that the localization process fails. This especially happens, if the robot is set into one of the main corridors of the basic scenario. Further, if the count of drawn particles is too high, the solution can not be calculated in real time. In contrast, if not enough particles are drawn from the current state of the localization, the solution of the algorithm yield to wrong exploration actions. Currently, 20 particles are drawn, which is slightly not enough to get a good representation of the current state to choose the best exploration action with the proposed algorithm for the active localization. For 20 particles, the time for calculation is more than five minutes. The size of the search grid can also be a problem for the real robot. To achieve a better solution, the size of the search grid has to be increased.

90 Chapter 5. Results and Evaluation Active Exploration Simulation with Gazebo To test the exploration strategies, a simulation with Gazebo is created. Figure 5.7 shows the complete 3D model of the Volksbot RT3 and visualizes the simulated laser scanner data. Figure 5.7: Complete 3D model of the robot with simulated sensor data for odometry and laser scanner, using Gazebo. The blue shaded shows the perception of the simulated laser scanner. The discontinuity of the scan at the right wheel is effected by the chassis of the robot. In figure 5.8 below, the 3D model of the robot is shown, interacting with the 3D model of the Institute of Cartography and Geoinformatics floor at the Leibniz Universität Hannover. It can be seen that the physical engine can generate the laser scan correctly. Figure 5.8: Combination of the 3D models from the robot and the basic scenario, using Gazebo.

91 Chapter 5. Results and Evaluation Evaluation of Exploration Strategy - Frontier Detection The exploration has been started from different positions in the basic scenario, as illustrated in figure Figure 5.9 shows the result of the exploration for starting the robot at start location with label 3. The different determined exploration targets are labeled with numbers in chronological order. Red dots correspond to not accessible exploration targets (near to obstacles), green dots to accessible and the blue dot is the start position. Figure 5.9: Result of the exploration, starting the exploration by means of simulation the with 3 labeled start position. The figures 5.10(a) to 5.10(g) illustrate exemplary for the first exploration actions, how the frontier detection based exploration strategy works for starting the exploration at start location labeled with 3. The blue dots are the current position of the robot, the green one is where the robot wants to move next. Red dots correspond to not accessible targets.

92 Chapter 5. Results and Evaluation 77 (a) Initial situation. (b) Map after the initial turn. The robot wants to drive to exploration target 1. (c) Robot reached 1 successfully and wants to drive to exploration target 2, because it is the frontier region with the minimal Euclidean distance. (d) Robot reached exploration target 2 successfully and want to drive to 3,which is not possible because this target is too near to a wall. (e) Aborting the navigation to 3, the next exploration target is 4, which is also quiet near to a wall and is not accessible, too. As can be seen, there are no changes on the explored map. (f) Still nothing new explored and the exploration target 5 is the next best target for the exploration. The robots starts to drive to 5. (g) Robot reached 5 successfully and searches for the next exploration target. Figure 5.10: Detailed illustration of the exploration for start position 3.

93 Chapter 5. Results and Evaluation 78 The results for the other start locations can be found in appendix D Active Exploration by Means of a Real Robot The Volksbot RT3 performs the algorithm for active exploration from the same start positions which have been already used by the simulation. During the experiments all offices except one are closed. Therefore, the robot can only explore the GIS-laborotory, as well as both main corridors and the corridor to the elevators. The upcoming figures 5.11 and 5.12 show the result of the exploration process exemplary for the start locations labeled with 1 and 3. All targets are labeled with a number, in a chronological order. Figure 5.11: Result of the exploration, setting the Volksbot RT3 at the with 1 labeled start position. The blue dot corresponds to the start position, the green to accessible exploration targets and the red one to not accessible.

94 Chapter 5. Results and Evaluation 79 Figure 5.12: Result of the exploration, setting the Volksbot RT3 at the with 3 labeled start position. The blue dot corresponds to the start position, the green to accessible exploration targets and the red one to not accessible. It can be seen that many exploration targets are not accessible in cluttered environments, but still the complete floor is explored. The results for the other start locations can be found in appendix D move base in Conjunction with GMapping Due to the fact that the map will be built up time by time, it can occur that the global path planning is sub-optimal. In particular, this problem corresponds to the point that the move base extends its global costmap for the path planning, if new segments of map are added. Problems arose while moving through doors based on the update of the global map by the GMapping node. It can happen that the robot gets stuck in the middle of a door, due to uncertainties stemming from map update, which is effected by uncertainties of the sensors Comparison of Different Active Exploration Strategies Criteria for Comparison In [42], different criteria are given and listed in the following:

95 Chapter 5. Results and Evaluation 80 Computational complexity: How much time the algorithm for the exploration requires to compute the next exploration target. Map and localization uncertainties: The uncertainties at the end of the exploration process. Shortest path: How much the robot has to move to explore the scenario. Covered map: How much of the scenario is explored by the strategy. Comparison The frontier-based and the information gain-based exploration strategy are greedy approaches. The next best view-based exploration strategy is a decision theoretic. Tables 5.2 and 5.3 summarize the advantages and disadvantages of the different strategies. Advantages Information Next-Best- Frontier-based Gain-based View-based Can explore environments containing cluttered and open space [20] Can explore environments where walls and obstacles are in arbitrary orientations [20] Efficient exploration by moving to targets which are most likely to gather as much new information as possible [20] Reduces the number of motions and sensing operations [10] Requires short path for exploration compared to the Next-Best-View-based strategy [43] Requires also short path for exploration [4] Easily extendable to multi-robot systems (described in [4, 587ff.]) Extendable to mulit-robot system by minor changes of the algorithm, if the relative poses of the robots are known [10] Table 5.2: Summary of advantages of the different exploration strategies.

96 Chapter 5. Results and Evaluation 81 Disadvantages Information Next-Best- Frontier-based Gain-based View-based Assumption that no sensing take place during movement to the exploration target [4] Not robust against errors during model building [10] Initially highly efficient. By growth of the map the exploration will be inefficient, due to alternating between different exploration targets [4] Due not alternate between exploration targets Explores some areas of the scenario not, due to the threshold for choosing a frontier region Table 5.3: Summary of disadvantages of the different exploration strategies. 5.4 Change Detection In the previous section 4.6.3, the robot has been set into a room. During the first run, there is no obstacle in the room, but during the second run, there is an obstacle. For the third run, only the location of the obstacle changed. The robot explored its environment and created the maps, which can be seen in figure (a) Recorded map, with no obstacle in the room. in the room. at different location in the room. (b) Recorded map, with obstacle (c) Recorded map, with obstacle Figure 5.13: Result of mapping process for change detection.

97 Chapter 5. Results and Evaluation 82 Using the introduced change detection method from section 2.8, the Kullback-Leiblerdivergence provides criteria for change detection. Checking the two recorded maps for a change, the change detector returns that a change in the environment has happened. The value of the Kullback-Leibler-divergence is calculated to When comparing the map of the second and third run with regard to changes, the change detector detects no change, i.e. D KL = 0.0. This problem can be explained as follows: Due to the change of the location, the absolute count of occupied, free and unknown cells does not change. The introduced method for change detection only detects changes, if some obstacles appear or disappear on the scene, but it is not robust against location-based changes. It is not robust against uncertainties in the map, either. This will lead to a detected change, although there is no change.

98 Chapter 6 Conclusion and Outlook In this thesis, one option to solve the exploration problem with a known map and a completely unknown indoor environment has been developed. Different strategies with their advantages and disadvantages have been discussed in this context. Last but not least, an implementation of a change detection method based on comparing maps of different mapping campaigns has been tested and evaluated. 6.1 Conclusion Can the robot localize itself without a system, which is able to determine the global pose? It has been shown that this challenge could be solved with the information gain and greedy based algorithm for the active localization with a given map using two particle clusters. Implementing this approach using a real robot shows that further research is required. Due to the computational complexity of the algorithm, a solution is currently not computable in acceptable time. Further it has been asked, where the robot should be moved to, to get a representation of the environment. The problem of exploration within a completely unknown environment is solved with the frontier-based approach by means of simulation and by use of a real robot. With the frontier-based exploration, the majority of the environment is explored. Further the implemented approach is compared with an additional greedy approach, which is based on the information-gain and a strategy based on decision theoretic criteria. The frontier-based approach requires a shorter path than the nextbest-view-based strategy. A problem of the frontier-based approach is the efficiency, which decreases with the growth of the explored map. The change detection is solved with a classification based method. The method uses 2D multi-class occupancy grid maps and as criteria the Kullback-Leibler divergence. The 83

99 Chapter 6. Conclusion and Outlook 84 2D multi-class occupancy grid maps can be reduced on grid maps with the three classes of occupied, free and unexplored. However, this approach is not very robust against changes in the environment. When the location of an object changes, the absolute count of the classes has an effect on the criteria. In particular, if only the location of the object changes and therefore, the count of occupied, free and unexplored cells is the same no change is detected. 6.2 Outlook To resolve the problem of the high computational complexity, the algorithm will have to be optimized in order to allow active localization of a real robot. One option is to optimize the speed of the Bayes filter step. One possible way is to implement the AMCL on a GPU (graphics processing unit) and parallelize the calculation of the motion and measurement model for every particle [44]. Further, the online and offline calculations can be split on to two separated computers, which is feasible with ROS. In particular, the AMCL, move base, all sensor nodes and RVIZ will still run on the computer, which is connected with the sensors of the robot. The complete computation of the active localization algorithm will run on a second computer with higher performance. In this thesis, three different approaches for active exploration have been discussed and the frontier-based approach has been implemented. Further, the greedy informationbased and the next-best-view-based strategy fit to be implemented. It is recommend to execute a series of experiments in order to evaluate the most efficient strategy. Helpful for the experiments could be the criteria given in section Also, other strategies exist. Examples for information gain based strategies are proposed in [42], [45] or [46]. A behavior based strategy is introduced in [47], strategies based on environmental boundaries are discussed in [48]. However, there are many more strategies. Last but not least, the frontier-based exploration strategy can be implemented more efficiently with some modifications, described in [27] and [43]. Currently, the simulation contains only a simple model of the real world. To make the simulation more realistic it can be extended by furniture at the office rooms and the laboratories, as well as randomly opened and closed doors. Additional to that, people can be modeled, too. The people can move through the simulated environment, given by precomputed trajectories or by a random walk. The huge advantage of a more realistic simulation is that complex algorithms for dynamic environments can be checked without any drawback. Using further sensors, like 2D or 3D cameras or a 3D laser scanner, it will be possible to implement a more robust and feature-based change detection. In [49] a method is proposed, which detects changes fully automatically at point level for 3D laser scanner.

100 Chapter 6. Conclusion and Outlook 85 Further, using a camera, well known methods for change detection from remote sensing could be adapted for mobile robots and the proposed method in this thesis can be adapted to a multi-class problem, by classification of the 2D scan points. Further, all of the described strategies to solve the exploration problem, can be applied to multi-robot systems, e.g. as proposed in [4] or [50], which describe a method for a coordinated exploration with a multi-robot system for active exploration and [51] for a Markov decision process based active localization with a multi-robot system.

101 Appendix A Description and Configuration of AMCL Parameters A.1 Description of the Parameters of the AMCL In the following, all important parameters, related to the different models for motion and measurement are listed: min particles: This parameter specifies the minimal number of particles used to represent the current state. Once a pose is estimated, a small number of particles are enough to track the robot s pose. max particles: This parameter specifies the maximal number of particles used to represent the current state. For the global localization the whole map has to be covered well that the pose can be estimated. kld err and kld z: kld err is the maximum error between the true distribution and the estimated distribution and kld z is the upper standard normal quantile for (1-p). p is the probability that the distribution estimate is less than kld err [37]. update min d and update min a: These both values describe the threshold, how much the robot has to move forward or rotate before an update will be applied. resample interval: It describes after how many update steps the resampling will be done. If the resampling is performed too often, the effect of particle deprivation (see section ) may lead to an erroneous localization. 86

102 Appendix A. Description and Configuration of AMCL Parameters 87 recovery alpha slow: This parameter is the short-term average of the measurement likelihood [4]. recovery alpha fast: In contrast, this parameter is the long-term average of the measurement likelihood [4]. laser model type: Via this parameter, the two different models discussed in section can be chosen. Possible models are likelihood fields (likelihood field) and the beam model (beam). laser max beams: This parameter describes how many beams will be used to calculate the likelihood during the update step. Therefore, every i-th beam will be used, where i is denoted by the quotient of maximum number of beams and the count of used beams. If the count of used beams is too high, the real time constraint is not fulfilled. laser z hit: This factor is the weight for the model, how much the measurement will be weighted to be correct. laser z rand: This factor describes, how much random measurements are weighted in the model. laser sigma hit: This determines the standard deviation of the laser scanner. laser likelihood max dist: Describes the inflation of obstacles onto the precomputed likelihood field. odom model type: Specifies the model for the odometry. For a differential odometry set diff and for an omni directional odometry, the value would be omni. odom alpha1: This parameter specifies the error of the rotational part of the movement due to a rotation. odom alpha2: This parameter specifies the error of the rotational part of the movement due to a translation. odom alpha3: This parameter specifies the error of the translational part of the movement due to a translation. odom alpha4: This parameter specifies the error of the translational part of the movement due to a rotation. The parameters to describe the initial state (mean and covariance) of the AMCL will be ignored for the global localization problem (see also: section ). If omni would be chosen for the odometry type, an additional parameter, α 5 has to be set. In the case of using the beam model, additional parameters for the different weights have to be set.

103 Appendix A. Description and Configuration of AMCL Parameters 88 A.2 Determination of Parameters The different factors to model the uncertainties, which were provided here, are only valid for the basic scenario at the Institute of Cartography and Geoinformatics floor of the Leibniz Universität Hannover (described in section 4.1). Using the robot at other locations, these parameters values can differ, because of other natures of the grounds. As precondition for the determination of the different parameters, the following parameters are fix: kld err = 0.05 kld z = 0.99 laser max beams = 30 laser sigma hit = 0.1 odom model type = diff min particles = 100 max particles = 5000 Choice of Measurement Model To decide which model is more likely to use, the robot is set into the GIS-laboratory and an update step of the AMCL is triggered. The resulting likelihoods are shown in figure A.1 and figure A.2. Figure A.1: Result of the update step for the beam model.

104 Appendix A. Description and Configuration of AMCL Parameters 89 Figure A.2: Result of the update step for the likelihood field model. It can be seen that the likelihood field model is more robust in the cluttered environment than the beam model. The likelihoods around the peak decrease not so fast than using the beam model. Therefore, a more better localization with the beam model is able. Determination of alpha 1 and alpha 2 To determine the two error parts of the odometry caused by rotational errors, the bidirectional square path experiment has been performed. Due to the fact that the basic scenario has no room, where the experiment can be performed, the robot can not drive four meters forward as proposed in [34]. Instead, only 1.50 meters are driven forward. Figure A.3 shows the start pose and the end poses of the robot for the different runs. Figure A.3: Result of the bi-directional square path experiment. Tables A.1 and A.2 summarize the results of the clockwise and counterclockwise run.

105 Appendix A. Description and Configuration of AMCL Parameters 90 Run absolute [cm] in x-direction calculated [cm] in x-direction absolute [cm] in y-direction calculated [cm] in y-direction 1 25,5-4,43 26,3-1, ,4 16-6, ,43 29,3-1, ,1-6,79 18,1-6,53 Table A.1: Summary of the results for the clockwise experiment. Absolute is the measured error in real. Calculated is the data provided by the odometry. Run absolute [cm] in x-direction calculated [cm] in x-direction absolute [cm] in y-direction calculated [cm] in y-direction ,6-37,4 8, ,3-7,4-29,6 6, ,8 0,12-23,7 9, ,4-2,4-20,8 8,84 Table A.2: Summary of the results for the counterclockwise experiment. Applying the equation of the benchmark on the recorded data, the parameter α = alpha 1 = 0, 0169 and β = alpha 2 = 0, Determination of alpha 3 To determine the factor α 3 the following experiment will be performed: The robot will be moved 1.20 meters forward for ten times. Table A.3 summarizes the data of the experiments. Nr. Absolute position in x-direction [m] Calculated position in x-direction [m] x [m] Table A.3: Summary of the results for the counterclockwise experiment. Therefore, the variance is determined to 0.047m 2.

106 Appendix A. Description and Configuration of AMCL Parameters 91 Determination of laser z hit and laser z rand After the model for the odometry is set, the model for the laser scanner has to be determined. Due to the fact that the likelihood field model is not a physical based model, the values of laser z hit and laser z rand for the AMCL have to be empirical determined. For the determination, the robot was set to the location, labeled with 1 (see figure A.4) and initialized with a pose and a covariance via rviz. After that, the robot is moved through the basic scenario. Figure A.4: Start pose and driven trajectory for determination of the likelihood field model parameters for the AMCL. This experiment is done for different combinations of the two parameters, starting by the default values, given in the ROS Wiki for the AMCL [37]. Up from laser z hit=0.95 and laser z rand=0.05, the parameter laser z hit is decreased by 0.05 and laser z rand is increased by 0.05, until laser z hit equals laser z rand (both are set to 0.5). The experiments show that the combination of laser z hit=0.9 and laser z rand=0.1 gives the best results for the localization process. Determination of update min a and update min d For the determination, the robot is set at the same location as described before and moved through the basic scenario. For both values, the proposed default values are used. At first, update min d has been increased by 0.05 from default value of 0.2 and update min a does not changed. After that, update min d has been increased again by 0.05 and so on. It was also decreased by To check, which is the best value for update min a, it has been decreased by 0.05 three experiments in a row and increased by 0.05 for further three experiments. The experiments show that the combination of update min d = 0.25 and update min a = 0.5 yield to good localization results in combination with model parameters for the odometry and the laser scanner.

107 Appendix A. Description and Configuration of AMCL Parameters 92 Determination of resample interval To determine how often the AMCL should perform a resampling, the AMCL get an initial pose as illustrated in figure A.4 and the robot is moved through the basic scenario. At first, the resampling interval has been set to one. The resampling interval will be increased by one, until the pose can not be tracked any more. The experiments show that a resampling interval of 2 yields to the best localization results. Determination of min particles During all experiments described above, the AMCL is able to track the pose with 100 particles as a lower bound. Determination of max particles To determine the count of maximal particles, the AMCL will be initialized with 5000 particles (default value, given by ROSWiki [37]) and it will be checked, if the pose can be determined by the AMCL. After that, the count of particles will be decreased by 500, until the global localization problem can not be solved any more by the AMCL. For every new value of max particles the robot is set in the middle of the corridors and in the GIS laboratory. This is shown in figure A.5. Figure A.5: Start locations to check which are maximal required particles to solve the global localization problem. The experiments show that 5000 particles cover the map enough to solve the global localization problem. If this amount is lower, the AMCL is not able to solve the ambiguities of the basic scenario.

108 Appendix A. Description and Configuration of AMCL Parameters 93 Determination of recovery alpha slow and recovery alpha fast The following experiment will be performed: The AMCL gets an initial pose and the robot will be set to a location, which differs from the initial pose. Then the robot starts to move and tries to determine its pose again. For the experiments, the default values for the decades will be used. The decades will be decreased by for recovery alpha slow and for recovery alpha fast, until the robot is able to recover from the localization error in an acceptable time. The experiments show that the robot will recover from the localization error with the combination of recovery alpha slow = and recovery alpha fast = 0.1. A.3 Configuration of AMCL Parameter Value Why chosen the specific value? min particles 100 See appendix A.2 max particles 5000 See appendix A.2 kld err 0.05 Proposed in Probabilistic Robotics [4] kld z 0.99 Proposed in Probabilistic Robotics [4] update min d 0.25 See appendix A.2 update min a 0.5 See appendix A.2 resample intervall 2 See appendix A.2 recovery alpha slow See appendix A.2 recover alpha fast 0.1 See appendix A.2 laser model type likelihood field See appendix A.2 laser max beams 30 Proposed by the ROSWiki [37] and the negative effect of correlated beams can be avoided (see section 2.4.2). laser z hit 0.90 See appendix A.2 laser z rand 0.10 In addition to laser z hit, this weight will be set to 0.10, because the sum of both weights has to be one. laser sigma hit 0.1 For the LMS100, this is determined to 60 millimeters in section But this is set to 0.1, due to the following effects: The standard deviation of the laser scanner is determined within perfect conditions. In the real world, the walls are not continuous or glass returns erroneous measurements. Secondly, using the recorded map, a discretization error will be done, which is uniformly distributed over the size of the cell. laser likelihood dist 2.0 Works well with the proposed value odom model type diff The Volksbot RT3 has a differential odometry. odom alpha It seams to be that the implemented motion model acts with something like a standard deviation. Therefore, odom alpha 1 is Not to underestimate the error and to keep the correct balance between the prediction and the update step, this value is set to 0.2. odom alpha As for alpha odom 1, the standard deviation is required and is 0.12 and set to 0.2 to keep the filter in balance. odom alpha The standard deviation is determined to 0.22, but set to 0.5 to keep the filter in balance. odom alpha This error is very close to zero, but as before, not to underestimate the error and keep the filter in balance, this is set 0.1. Table A.4: Summary of the configuration of the AMCL.

109 Appendix B Description and Configuration of move base B.1 Description of the move base The move base is a node of the ROS distribution, which provides a tool for autonomous navigation in dynamic environments. Figure B.1 shows the different subtasks of the move base and how they interact with each other. Figure B.1: Summary of the move base and how the different subtasks of the move base interact with each other. Source of figure: base If the move base receives a new goal, it plans a path from the current pose of the robot to the goal pose. This global plan will be calculated using the Dijkstra algorithm. The planning of the global path is based on the information of the global costmap, which forces the planned path away from obstacles. The global plan will only change, if the 94

110 Appendix B. Configuration of move base 95 local path planner notifies that an obstacle is in front of the robot or the global map changes (just by using the move base in conjunction with GMapping). The local path planner determines the velocity commands to execute the navigation, based on its local costmap, which also detects obstacles in the environment. For the case that the robot gets stucked, the different recovery behaviors are executed. Figure B.2 shows in which order the behaviors are executed. Figure B.2: Summary of the executed behaviors to unstuck the robot. figure: base Source of At first, the local costmap will be cleared and built up new from this point. After that, the robot can navigate again. If not, the robot performs a 2π turn in place and build up the costmap new. The aggressive reset clears both costmaps. If the navigation fails after the second turn in place, the path planning will be aborted (something blocks the robot, or the target is within the inflated area of the costmap) and the robot waits for the next target. B.2 Configuration of move base The next sections explain how to configure the different components of the move base to use it in different global coordinate frames. B.2.1 base local planner The base local planner is package of ROS, which provides a controller to move a robot in the plane [37]. The important parameters are given in the upcoming listing: holonomic robot: Specifies, whether the robot can perform movements in the y-direction. In this thesis, the Volksbot RT3 is not able to perform movements in y-direction. Therefore, this parameter is set to false for every configuration. yaw goal tolerance: The tolerance for the heading, given in radians. xy goal tolerance: component. The tolerance for the position, given in meters for every

111 Appendix B. Configuration of move base 96 sim time: Defines the decade for forward-simulation of the trajectory. pdist scale: Defines the weight how much the robot should follows the global path. gdist scale: Defines the weight how much the robot should reach its calculated control actions. global frame id: The frame to set the costs. Should be the same as for the local costmap. The base local planner is configured as shown in table B.1. Parameter Value Why chosen the specific value? yaw goal tolerance 0.2 To handle uncertainties during acquisition of the map. xy goal tolerance 0.3 Set to a big value, to handle uncertainties during acquisition of the map. If set to a smaller value, the robot can not turn in place. sim time 3.0 The higher the value, the more accurate the robot can follow the path. Can not set much higher, because this would yield to real time calculation problems. pdist scale 1.5 The value of this parameter is set bigger then the value of gdist scale to force the robot on the global planned path. gdist scale 0.8 Could not be zero, to navigate the robot safely through doors. global frame id odom combined This frame is specified as global frame for the costmap. Table B.1: Listing of the important parameters and their values for the configuration of the base local planner. B.2.2 Costmap Parameters The upcoming listing explains the important parameters for the costmaps [37]: global frame: Defines the frame in which the costmap operates in global context. robot base frame: Defines the base frame of the robot. transform tolerance: Defines the maximal delay of a transformation between the frames. update frequency: The frequency for updating the map.

112 Appendix B. Configuration of move base 97 publish frequency: The frequency for publishing the map for displaying it in rviz. obstacle range: The maximum distance from the to an obstacle, where the obstacle will be inserted into the map. raytrace range: The maximum distance to remove obstacles from the map. cost scaling factor: Factor for scaling costs during inflation. inflation radius: Radius around an obstacle for inflating it on the map with costs. footprint: Defines the principle physical properties of the robot. robot radius: Radius of the robot. Can be calculated as follows: x 2 + y 2, where x is the length of the robot and y is the width of the robot. observation sources: Defines a list of sensor sources for observing the environment. observation source/topic: The name of the topic, where the observation source publishes its data. observation source/sensor frame: The frame of the sensor data. Required for transformation. observation source/expected update rate: The expected update rate of the sensor. Best way to determine is that via the tf monitor and using the average time and additionally a buffer for it. observation source/data type: The type of the sensor data, e.g. laser for a laser scanner. observation source/marking: Specifies, whether the sensor can add obstacles to the costmap. observation source/clearing: Specifies, whether the sensor can clear obstacles from the costmap. static map: Specifies, whether a costmap is static or not. rolling window: If static map is set to true, this parameter has set false. map topic: The name of the topic, where the costmap can subscribe the static map. width: The width of the costmap, given in meters.

113 Appendix B. Configuration of move base 98 height: The height of the costmap, given in meters. resolution: The resolution of the costmap, given in pixels per meters. origin x and origin y: The origin of costmap in x-direction and y-direction. map type: The type of the map. Set to costmap in every configuration of this thesis. track unknown space: Specifies, whether to track cells, which never observed yet. B Costmaps for Active Exploration In the case of exploration, the occupancy grid map will be build up succesivly by the GMapping node. Therefore, the move base have to be configured in the way that it is able to run in conjunction with the GMapping node. Local Costmap Parameter Value Why chosen the specific value? global frame /odom combined Global frame for navigation in local context. Uses the odometry. robot base frame /base footprint Defined frame of the robot. See transformation tree in figure 4.9. inflation radius 0.3 Has to set minor than the inflation radius of the global costmap that the robot is able to drive through doors, due to uncertainties during the process of mapping. update frequency 5.0 High update rate of the local costmap to be sure that dynamic obstacles are perceived as soon as possible and that the local costmap is updated as soon as possible, if the map provided by the GMapping node is updated. publish frequency 1.0 Publishes the local costmap every second for the scope of visualization. Will not be done more often to avoid overload of the system. static map false Non static map, due to the fact that this map is used for the local field of view of the robots sensor. rolling window true Have to be set true, because the parameter before is set to false. width 2.5 Using a minor value to avoid overload of the system and to handle uncertainties arose by using the move base in conjunction with GMapping. height 2.5 Same as before for the width of the costmap. resolution The GMapping node records the map with a cell resolution of 0.05 and the resolution of the global costmap has to be more accurate. origin x / origin y 0.0 The origin of the global costmap is the origin of the map. Table B.2: Summary of the parameters for the local costmap in context of the active exploration task.

114 Appendix B. Configuration of move base 99 Global Costmap Parameter Value Why chosen the specific value? global frame robot base frame /map /base footprint inflation radius 0.9 update frequency 3.0 publish frequency 1.0 static map rolling window false true width 50.0 Global frame, by using GMapping and navigation in this frame. Defined frame of the robot. See transformation tree in figure 4.9. Results from x 2 + y 2. Forces the global planner to plan the path in middle through doors. The GMapping node publishes a map after one meter translational movement or 0.5 radians rotational movement. Therefore, this is slightly enough to update the global costmap. Publishes the global costmap every second for the scope of visualization. Will not be done more often to avoid overload of the system. Non static map, due to the fact that the move base runs in conjunction with GMapping, which updates the map. Have to be set true, because the parameter before is set to false. Using big value, to be sure that the path planning is consistent over the whole map. height 50.0 Same as before for the width of the costmap. resolution origin x / origin y 0.0 The GMapping node records the map with a cell resolution of 0.05 and the resolution of the global costmap has to be more accurate. The origin of the global costmap is the origin of the map. Table B.3: Summary of the parameters for the global costmap in context of the active exploration task. Common Costmap Parameters Table B.4 shows the important parameters for the costmap, which are valid for the global and the local costmap. Parameters which differs are listed in the paragraphs before.

115 Appendix B. Configuration of move base 100 Parameter Value Why chosen the specific value? transform tolerance 0.75 Is set to 0.75 to handle problems with system overload. obstacle range 3.75 Set higher than the hight and width of the local costmap, to be sure that all obstacles are perceived for the local path planner. raytrace range 4.0 Set higher than the obstacle range. footprint [ [0.125, ], [-0.455, ], [-0.455, ], [0.125,0.260], See definition of the footprint in figure 4.2. [0.250, 0.05], [0.250, -0.05] ] observation sources scan Identifier for the defined observation source. Here scan refers to the use of the LMS100. The LMS100 publishes its data on the topic observation source/ scan. Due to the fact that the robot /scan filtered topic perceives its own chassis, the filtered topic will be used. observation source/ This name is defined by the driver for the /lms100 sensor frame laser scanner for the frame. observation source/ expected update rate observation source/ data type observation source/ clearing observation source/ marking 0.5 LaserScan true true unknown cost value 0 This value is determined using the tool tf monitor with an additional buffer to be sure that this update rate will be observed. Otherwise the move base will not command the robot. Has to be set to LaserScan, because a laser scanner is used. The source is allowed to clear obstacles from costmaps, because it is the only observation source. The source is allowed to add obstacles to costmaps, because it is the only observation source. If the state of the cell is unknown, i.e. the cell is unexplored, allow the planner to plan a valid path through this unexplored terrain. This is important to run the move base in conjunction with GMapping. Table B.4: Summary of the important parameters and their chosen values for the costmap parameters, which are the same for the global and the local costmap. B Costmaps for Active Localization The following section describes how the parameters of the move base are set to run it in the context of navigation in the local coordinate frame, due to the fact that during the active localization, the global pose is unknown. Local Costmap The used parameters for the local costmap in context of the active localization are the same as for the active exploration, which are listed in table B.2.

116 Appendix B. Configuration of move base 101 Global Costmap For the global costmap, only the global frame changes to odom combined. This has to be done due to the fact that the absolute pose in the map is unknown during the active localization. The rest of the parameters are the same and can be found in table B.3 Common Costmap Parameters The used parameters are shown in table B.4.

117 Appendix C ROS C.1 Own Created ROS Nodes Util This package contains different base classes, e.g. for any probabilistic filter. It also contains classes, which provide different operations and functions that all other nodes are use. Additionally, the different sensor and related operations are modeled by own classes and test classes are available. ActiveLocalization This package contains the classes, which implement the functionality of the active localization as well as a main function for the node. Also, a service is defined, where the active localization can be called from every other node. The parameters for running the node are: sigma xx term: Specifies the accuracy in the x-component for the localization. sigma yy term: Specifies the accuracy in the y-component for the localization. sigma aa term: Specifies the accuracy in the heading for the localization. t term: Specifies after which time the localization process will be automatically terminated. By default, the active localization process will be terminated after 60 seconds. precompute ranges: Specifies, if the ranges for an occupancy grid map have to be pre-computed or not. False is set as default value. 102

118 Appendix C. ROS 103 Exploration This package contains the URDF files to create the interactive 3D simulation of the robot with Gazebo and a base class for an arbitrary exploration strategy. From this one, all other strategies are derived. Additionally, a class for exploration is created, which executes a specific exploration strategy and handles the navigation to the exploration target. Last but not least, a main class for the node is created, which starts the exploration process. The parameter for running the node is called strategy and defines which exploration strategy will be used. The default value of the node is frontier and specifies that the frontier based exploration strategy described in section will be used. ChangeDetection This package contains a base class for arbitrary change detectors and the implementation for the simple change detector, as described in this thesis. Due to the fact that no online version for the detector exists, only the main function has got parameters. These are described later in this section. C.2 Communication of ROS Nodes Active Localization Figure C.1: Communication the ROS nodes for active localization.

119 Appendix C. ROS 104 Gazebo Simulation and Active Exploration Figure C.2: Communication of the ROS nodes for simulation and autonomous exploration. Active Exploration in a Real Environment Figure C.3: Communication the ROS nodes for autonomous exploration executing on the Volksbot RT3.

120 Appendix C. ROS 105 C.3 Start the different Tasks The upcoming section describes which launch files have to be started in which order to execute the different algorithms with the Volksbot RT3 or as a simulation. Active Localization Starting the active localization, go to the folder launch in the stack gangl and execute the following command: $ roslaunch ActiveLocalizationLikelihoodField. launch This starts the driver for the LMS100 and the motor of the Volksbot RT3, as well as the map server to publish the map, the AMCL for localization and the node ActiveExploration to the localization process. It also starts the tool rviz for visualization. Gazebo Simulation and Active Exploration To start the active exploration with the Gazebo simulation, go to the folder launch in the stack gangl and enter the following command on the command line tool: $ roslaunch Simulation. launch This starts gazebo and spawns the model of the IKG and the Volksbot RT3 and simulates the topics for the laser scanner and the odometry. Additionally, this starts the tool rviz for visualization. The model of the robot will be spawned (initial positioning at the simulation) in the middle of the scenario. The upcoming table C.1 lists which x- and y-coordinates have to be set to spawn the robot at different locations of the scenario. If the spawn point is located on a wall, the robot flies through the environment and the simulation fails. In general, the spawn location can be changed in the file Simulation.launch. Just change the numbers behind x and y in the follwoing lines: $ <node name =" spawn_robot " pkg =" gazebo " type =" spawn_model " $ args ="-x 0.0 $ -y 0.0 $ -z 0.0 $ - unpause $ -urdf $ - param robot_description $ - model robot_description $ - ros_namespace / gazebo " $ respawn =" false " output =" screen " />

121 Appendix C. ROS 106 With ROS Groovy and Gazebo, this does not work at all. A work around for this problem is to change the origin of the model. To change the start location of the robot, change the first two entries of the tag pose (red labeled) at the include of the 3D model of the scenario gangl, using the values from table C.1. The file is located in the package Exploration and there at the folder world. $ <sdf version =" 1.3 "> $ <world name =" default " > $ <!-- The ground - plane --> $ <include > $ <uri > model :// ground_plane </ uri > $ </ include > $ $ <!-- Global light source --> $ <include > $ <uri > model :// sun </ uri > $ </ include > $ $ <!-- Load the 3d model of the IKG --> $ <include > $ <uri > model :// gangl </ uri > $ <pose > </ pose > $ </ include > $ $ <gui > $ < camere name =" user_camera " > $ <pose > </ pose > $ </ camera > $ </gui > $ </ world > $ </sdf > Location at scenario x-coordinate y-coordinate Middel of room to the elevators GIS labor End of right floor Office Table C.1: Summary of x- and y-coordinates to spawn robot at different locations Start the active exploration with the following command: $ roslaunch AutonomousExploration. launch This starts the nodes GMapping, move base (configured to use in conjunction with GMapping), Exploration and the node WallNotifier to abort the navigation if targets are quiet close to a wall. To save the recorded map, run the map saver tool of the map server with the following command: $ roslaunch SaveMap. launch

122 Appendix C. ROS 107 The recorded map will be saved in the folder map of the stack gangl with the filename exploredmap.pgm and the corresponding configuration file exploredmap.yaml. Important is to run the last launch file, before stopping the execution of the other started launch files, otherwise the map can not be saved. Active Exploration without Simulation To start the active exploration for a real robot, go to the folder launch in the stack gangl and enter the following command on the command line tool: $ roslaunch AutonomousExplorationRT3Real. launch This starts the driver for the LMS100 and the motor of the Volksbot RT3, as well as the GMapping and the Exploration node. It also starts the tool rviz for visualization. After exploration, the recorded map can be saved, starting an additional launch file: $ roslaunch SaveMap. launch The recorded map will be saved in the folder map of the stack gangl with the filename exploredmap.pgm and the corresponding configuration file exploredmap.yaml. Important is to run the last launch file, before stopping the execution of the other started launch files, otherwise the map can not be saved. Change Detection To create the different maps, start the following launch files twice: $ roslaunch ChangeDetectionSimulation. launch $ roslaunch AutonomousExploration. launch $ roslaunch SaveMap. launch Before starting the launch file AutnomousExploration.launch during the second run, spawn one ore more obstacles in Gazebo. To start the change detection, no launch file is currently defined, due to the fact that no online version of this method exists. Therefore, enter the following to the command line give the path to the maps for the parameters path map t0 and path map t1: $ rosrun ChangeDetection Detector path_map_t0 path_map_t1

123 Appendix C. ROS 108 C.4 Add Model to Gazebo The following steps describe, how to add a created 3D model can be added to the environment of Gazebo with ROS Groovy: 1. Go to the home folder and then change to the folder./.gazebo/models/ 2. Add a new folder and jump into it 3. Add the folder meshes and add the 3D model file to the folder meshes 4. Create the file model.config and add the following lines of code from the first listing below and change the red marked text to the concrete case 5. Create the file model.sdf and add the following lines of code from the second listing below and change the red marked text to the concrete case $ <? xml version =" 1.0 "?> $ <model > $ <name > </name > $ <version >1.0 </ version > $ <sdf version =" 1.3 ">model.sdf </ sdf > $ $ <author > $ <name >... name here... </ name > $ < >... here... </ > $ </ author > $ $ <description > $... description here... $ </ description > $ <model > $ <? xml version =" 1.0 "?> $ <sdf version =" 1.3 "> $ <model name ="... name here..."> $ <static >true </ static > $ <link name =" walls "> $ < collision name =" collision " > $ <geometry > $ <mesh > $ <uri > model ://... path up from.gazebo/models here...</uri > $ </mesh > $ </ geometry > $ </ collision > $ < visual name =" visual "> $ <geometry > $ <mesh > $ <uri > model ://... path up from.gazebo/models here...</uri > $ </mesh >

124 Appendix C. ROS 109 $ </ geometry > $ < cast_shadows > false </ cast_shadows > $ </ visual > $ </link > $ </ model > $ </sdf >

125 Appendix D Supplementary Figures D.1 Results of the Active Localization Figure D.1: Result of the greedy search for the next best exploration action to execute to determine the pose of the robot for scenario shown in figure The more red a cell of the search grid, the more likely it is to choose this exploration action. For this experiment, the exploration action a exp = ( 1.0m, 0.0m) would be chosen. 110

126 Appendix D. Supplementary Figures 111 Figure D.2: Result of the greedy search for the next best exploration action to execute to determine the pose of the robot for scenario shown in figure The more red a cell of the search grid, the more likely it is to choose this exploration action. For this experiment, the exploration action a exp = ( 5.0m, 0.0m) would be chosen. D.2 Results of the Exploration by Means of Simulation The red dots are not accessible target, because these exploration targets are close to walls or other obstacles. Green dots are successfully reached exploration targets and the blue dot is the start position of the exploration. Figure D.3: Result of the exploration by means of simulation, setting the Volksbot RT3 at the with 1 labeled start position.

127 Appendix D. Supplementary Figures 112 The position with label 30 equals the start position. After loop closing, additional exploration targets were available. Figure D.4: Result of the exploration by means of simulation, setting the Volksbot RT3 at the with 2 labeled start position. The same as before, after driving back to the origin of the exploration (position with label 31 ) new exploration targets are available. Figure D.5: Result of the exploration by means of simulation, setting the Volksbot RT3 at the with 4 labeled start position.

128 Appendix D. Supplementary Figures 113 D.3 Results of the Exploration by Means of a Real Robot The red dots are not accessible target, because these exploration targets are close to walls or other obstacles. Green dots are successfully reached exploration targets and the blue dot is the start position of the exploration. Figure D.6: Result of the exploration, setting the Volksbot RT3 at the with 2 labeled start position.

129 Appendix D. Supplementary Figures 114 Figure D.7: Result of the exploration, setting the Volksbot RT3 at the with 4 labeled start position.