UNIVERSITÀ DEGLI STUDI DI GENOVA MASTER S THESIS

Transcription

1 UNIVERSITÀ DEGLI STUDI DI GENOVA MASTER S THESIS Integrated Cooperative SLAM with Visual Odometry within teams of autonomous planetary exploration rovers Author: Ekaterina Peshkova Supervisors: Giuseppe Casalino Enrico Simetti July 2013

2 ABSTRACT The problem of the multi-agent SLAM exploration is considered in this thesis. The goal of this work is to devise a comprehensive approach for Cooperative SLAM Exploration implemented together with Visual Odometry. A review of the state of the art has shown that there exist various techniques that deal with the problem of exploration. However, the vast majority of proposed coverage techniques assume that the agent s pose is known and this significantly simplifies the task. In the exploration of unknown environment, the absolute localization is not available. At the same time, the information provided by sensors contains noise and, as a result, the true position of the agent and landmarks are uncertain. In the last decades, the Simultaneous Localization and Mapping (SLAM) algorithm has proved to be a powerful tool against noise introduced by sensors. However, the applicability of SLAM for large environments is problematic due to its high computational cost. Therefore, the recently proposed Sparse Extended Information Filter SLAM that has quasi-constant computational cost and whose potential remains poorly investigated, was selected as a basis for the exploration mission. The second key reason of this choice is its natural extension to the multi-agent system. The efficient strategy of where to navigate the agent to speed up the exploration mission while building an accurate map is described based on state of the art techniques. Its extension to the multi-agent case is provided for both systems with unlimited communication range and more realistic ones that take into account the communication bound. The exploited Odometry technique influences significantly the overall SLAM performance. An efficient Odometry leads to faster exploration and contributes to the construction of an accurate resultant map of the environment. In last decades, Visual Odometry technique has shown its reliability as it provides highly accurate results of the agent s motion estimation. The accuracy of the obtained map has a crucial importance for planetary exploration missions. Therefore, Visual Odometry is chosen instead of standard Wheel Odometry that is influenced by the wheel slip in the rough terrain. A possible improvement of the Visual Odometry technique is proposed that leads to smaller error of the motion estimation at a price of additional computations. 2

3 CONTENT LIST OF FIGURES... 5 LIST OF TABLES... 7 INTRODUCTION SLAM General Overview SLAM solutions Extended Kalman Filter SLAM Fast SLAM Computational complexity and data association Sparse Extended Information Filter SLAM Preliminaries Description of Sparse Extended Information Filter SLAM Simulation results Implementation of EKF SLAM and EIF SLAM Implementation of SEIF SLAM Overconfidence of SEIF SLAM Extension to Multi-agent SEIF SLAM ACTIVE SLAM Coverage techniques Unlimited communication range Limited communication range Active SLAM for a single-agent case Single-agent Active SLAM algorithm Simulation results Improvements of the single-agent Active SLAM algorithm Simulation results Active SLAM for a multi-agent case

4 Multi-agent SLAM algorithm with unlimited communication range Simulation results Multi-agent SLAM algorithm with limited communication range Simulation results VISUAL ODOMETRY General Overview Proposed improvements CONCLUSIONS AND FUTURE DEVELOPMENTS ACKNOWLEDGMENTS REFERENCES

5 LIST OF FIGURES Figure 1 The EKF SLAM algorithm Figure 2 Derivation of the canonical parameterization Figure 3 Comparison of information matrices Figure 4 The SEIF SLAM algorithm Figure 5 Motion Update Step (SEIF SLAM) Figure 6 Simulation of the SEIF SLAM algorithm (the motion update step) Figure 7 Effect of the Motion Update Step on the Information Matrix Figure 8 Measurement Update Step (SEIF SLAM) Figure 9 Simulation of the SEIF SLAM algorithm (detection of the new landmark) Figure 10 Effect of the Measurement Update on the Information Matrix Figure 11 Simulation of the SEIF SLAM algorithm (the sparsification step) Figure 12 Effect of the Sparsification Step on the Information Matrix Figure 13 Sparsification Step (SEIF SLAM) Figure 14 Effect of the noise in the motion and the measurement models Figure 15 Effect of the loop closure on the uncertainties of landmarks Figure 16 Effect of loop-closure on the agent s pose uncertainty Figure 17 Correction of the agent s pose ( loop-closure ) Figure 18 CPU time Figure 19 Result of the Maps Collapse Figure 20 An example of the occupancy grid map Figure 21 Role-based exploration algorithm Figure 22 Sector-search algorithm Figure 23 Coordination of agents Figure 24 Active loop-closing technique Figure 25 Three modes of the exploration technique Figure 26 Deployment of markers Figure 27 Algorithm of the adapted single-agent SLAM exploration technique Figure 28 Decrease of the global map uncertainty Figure 29 Histogram of the noise distribution Figure 30 Distributed samples Figure 31 Effect of the threshold introduction Figure 32 Environment used for the simulation Figure 33 Stages of the snake-like exploration technique Figure 34 Exploration time Figure 35 Average map accuracy error Figure 36 Simulation environment for a multi-agent case

6 Figure 37 Simulated environment for a multi-agent case Figure 38 Independent exploration mode Figure 39 Rendezvous mode Figure 40 Main steps of Stereo VO Figure 41 Sequence of agent s frames Figure 42 Block diagram of the state-space model

7 LIST OF TABLES Table 1 Graphical representation Table 2 Comparison of EKF SLAM and EIF SLAM in terms of computations Table 3 Average error N Table 4 Average error N Table 5 Effect of the sparsification step on the average pose error Table 6 Effect of the approximate mean recovery step on the average pose error Table 7 Comparison of the EIF SLAM and the SEIF SLAM algorithms Table 8 Maps used for simulations Table 9 Comparison of coop-closure Table 10 Overconfidence of the SEIF SLAM algorithm Table 11 Entropy Table 12 Stages of the single-agent exploration Table 13 State of the map depending on the position confidence Table 14 State of the map depending on the number of samples N Table 15 State of the map depending on the number of samples N Table 16 Comparison of the time parameter Table 17 Comparison of the accuracy parameter Table 18 Resultant map uncertainty Table 19 Occupancy map confidence Table 20 Total number of steps (sensing range) Table 21 Total number of steps (motion uncertainty) Table 22 Effect of the threshold Table 23 Single-agent vs. Multi-agent exploration (threshold) Table 24 Single-agent vs. Multi-agent exploration (range) Table 25 Single-agent vs. Multi-agent exploration (uncertainty) Table 26 Performance comparison (time) Table 27 Number of meetings vs. speed of coverage

8 INTRODUCTION During the last decade, several exploration missions have been performed by single autonomous rovers. The success of exploration missions strongly relied on the efficiency and the reliability of the Visual Odometry (VO) technique that has been developed within more than one decade of research performed at international level. Nevertheless, as an agent moves, the error of the VO motion estimation constantly grows. When the travelled distance is short enough, the error introduced by VO can be neglected. However, for a large environment this error results in a significant drift of the estimated trajectory. Therefore, a mechanism that is able to reduce this error when the absolute localization is not available has to be used. SLAM that stands for Simultaneous Localization And Mapping is an algorithm that is aimed to build a map of the environment and to concurrently compute the agent s absolute pose while working in the unknown environment and using the information from noisy sensors. Therefore, the next step in the exploration field that would allow to overcome the described problem of the error accumulation is to integrate VO and SLAM techniques. Based on achieved success of the exploration mission, a strong interest is now directed to the possibility of exploiting the cooperation within a team of rovers for further enhancing the planetary exploration capabilities, both in terms of precision of resultant maps and in terms of the operational time. To this aim, a proper distributed algorithm allowing a high accuracy of the constructed map and a reduction of the time required for the exploration needs to be developed. A multi-agent SLAM exploration has several definite advantages over a single-agent SLAM exploration. Particularly, it provides higher speed of coverage, more efficient localization, more accurate resultant maps due to merging of overlapping information, and, in general, more fault-tolerant systems. However, there exist difficulties such as limited communication range and limited computational capabilities that have to be resolved. Consequently, a suitable algorithm with sufficiently low computational cost that efficiently coordinates agents while dealing with the problem of limited communication range has to be developed. The key problem to solve in multi-agent exploration is where to guide agents to speed up the process of area coverage. Many researches are focused on developing efficient algorithms for coverage and, as a result, various techniques have been proposed in the last decades. 8

9 COVERAGE TECHNIQUES WITH UNLIMITED COMMUNICATION RANGE The frontier-based exploration [1] proposed by Yamauchi is the core of a large majority of existing techniques. The principal idea of the frontier-based exploration is to guide agents to the closest frontier cell between known and unknown cells in order to obtain new information about the world. However, this approach does not ensure that several agents would not select the same frontier cell. Various techniques were proposed in order to deal with the problem of redundant coverage. In [2], it is suggested to introduce the utility function to find the optimal target cell. The utility of a given cell is defined by the expected information gain obtained upon reaching the frontier cell under study. Whenever a target cell is assigned to an agent, the utilities of adjacent cells are reduced. The distributed frontier-based algorithm proposed in [3] makes use of the Particle Swarm Optimization to avoid redundant coverage. COVERAGE TECHNIQUES WITH LIMITED COMMUNICATION RANGE However, all the described algorithms do not address the problem of limited communication range. More recently, several approaches that address this limitation have been developed. [4] is a static role-based exploration algorithm where each agent is assigned either a role of an explorer or a relay. The task of the former is to explore the farthest areas of the environment, while the task of the latter is to deliver knowledge obtained from an explorer to the command center. A meeting point is defined between an explorer and its relay before going out of the communication range. The extension of this algorithm to a dynamic role-base exploration algorithm was later proposed in [5] to improve the performance. Another recently proposed algorithm uses a sector search with rendezvous to coordinate a team of autonomous agents [6]. [9] aims to maximize the information about environment while minimizing the travelled distance such that each agent is within a communication range at least with one team member. [10] solves the problem of coordinating agents using the potential field based approach in which agents are attracted to unexplored area and are repulsed from obstacles and other agents. The problem of limited communication is solved by dividing a team of agents into smaller sub-teams and by introducing a leader for each sub-team that is within communication range with at least one member of other sub-teams. SLAM All the mentioned techniques assume the availability of absolute localization. Last decades have shown that SLAM is a very powerful tool that does not make the assumption of the known agent s localization. Therefore, an extension of a single-agent SLAM technique 9

10 to a multi-agent SLAM technique, based on developed multi-agent exploration strategies, is the most reasonable solution to the problems faced in the field of explorations. The state of the art of multi-agent SLAM exploration shows that this field is far from being fully explored and there are just few papers that address this problem. [11] is one of the recent ones that integrates a multi-agent exploration technique and the EKF SLAM. STRUCTURE OF THE THESIS Chapter 1 is dedicated to the detailed study of the problem of SLAM including an overview of the principal SLAM solutions for a single-agent system with particular attention to the aspect of computation cost. One of the key solutions of the SLAM problem is the well-known Extended Kalman Filter SLAM. However, EKF SLAM has very high computational cost and, therefore, cannot be applied for large environments. In this thesis, it is suggested to use the recently proposed Sparse Extended Information Filter SLAM [7] whose potential remains not very well explored. The principal advantage of SEIF SLAM is its quasi-constant computational cost and its straightforward extension to multi-agent systems. In Chapter 2, the Active SLAM algorithms for both single-agent and multi-agent systems are discussed. Based on the review of the state of the art, the appropriate algorithm is selected and proposed improvements are described together with the results of their simulations in MATLAB. Chapter 3 focuses on the Visual Odometry (VO) technique that is advantageous, in case of navigation within the rough terrain, compared to standard Wheel Odometry technique. After the brief description of the principal steps of VO, the improvement that possibly reduces the VO error at the price of additional computations is described. CONTRIBUTION The main contribution of this thesis is the development of a comprehensive Multiagent exploration technique that takes into account the agent s pose uncertainty and addresses the problem of limited communication. To achieve such a goal, Sparse Extended Information Filter (SEIF) SLAM is selected as a base for the exploration technique due to its advantageous quasi-constant computational time and its natural extension to a multi-agent system. The problem of limited communication is solved by exploiting the rendezvous strategy where agents are allowed to go temporarily beyond the communication range and then meet in previously agreed upon rendezvous points. The integration of the SEIF SLAM algorithm with the Visual Odometry technique that in the last decade has proved its reliability and efficiency is proposed in order to increase the accuracy of the resultant map. An improvement of the standard Visual 10

11 Odometry is suggested that makes use of the additional measurements in order to reduce the accumulated motion estimation error. 11

12 Chapter 1 1. SLAM This chapter is dedicated to the problem of SLAM and is subdivided into three main sections. Section 1.1 of this chapter provides a general overview of the SLAM problem. The major SLAM solutions such as the EKF SLAM and the Fast SLAM algorithms are briefly described. The major issues of the SLAM problem are discussed. Section 1.2 focuses on the SEIF SLAM algorithms. This section starts by giving some preliminaries of the Information Filter and the Extended Information Filter. Then, the detailed description of the SEIF SLAM algorithm and its mains steps are provided. In Section 1.3, the performance of the EKF SLAM and the EIF SLAM algorithms is analyzed based on results of simulations in MATLAB. Next, the behavior of the implemented SEIF SLAM algorithm is compared to the one of the EIF SLAM. More precisely, the effect of the sparsification step and the approximate mean recovery step on the agent s pose are examined. The comparison of the simulated loop closure is provided. Section 1.4 discusses the phenomenon, known as the overconfidence that is introduced by the sparsification. Section 1.5 of this chapter describes the technique of maps integration, so-called maps collapse, and the result of its implementation in MATLAB is provided General Overview The Simultaneous Localization and Mapping algorithm, known as SLAM, addresses the problem of localization and mapping concurrently. SLAM is applied in a wide range of environments. It can be considered as a solved problem for small environments with wellseparated and easily distinguishable landmarks. However, there exist many issues that have to be investigated for large unstructured environments where SLAM can truly contribute; one of them is space exploration that is addressed in this thesis. 12

13 The SLAM problem is defined as follows: an agent is placed in an unknown environment and its task is to build the map of the environment while determining its location within this map. The agent is equipped with a set of proprioceptive and exteroceptive sensors in order to cope with the formulated task. In the most general case, an agent has in-built encoders that provide the essential information that allows to apply a standard wheeled Odometry technique to compute an agent s pose that includes its orientation (yaw) and x,y coordinates of its position (2D case). A range and a bearing sensors that measure the distance to the landmark together with its bearing with respect to the agent s current frame constitute the most common model used to accomplish the second task of the SLAM problem mapping. Noise introduced by the imperfection of sensors makes the already difficult task of SLAM more complicated. An agent navigates in the environment while taking measurements of landmarks within the sensing range. At each step an agent possesses the following information: - a mean vector that contains an agent s estimated pose ( ) and estimated positions of detected landmarks ( ), - a control vector applied at a previous step ( ); - a vector of observations taken at step ( ). In probabilistic form, the solution of the SLAM problem is represented as probability distribution: Where an agent s initial pose. a sequence of all observations, a sequence of applied control inputs; (1.1.1) A probability distribution describes a joint posterior density of an agent s pose and landmarks positions given the initial state of an agent, a sequence of applied control inputs, and a sequence of taken observations. Computation of a joint posterior is based on Bayes rule. At any step, a previous joint posterior, a vector of control inputs, and a vector of taken measurements are known. The current agent's pose is assumed to depend only on the previous pose and the current control input. 13

14 The motion model is based on Odometry equations. The measurement model is defined by the sensor model. These models form the SLAM algorithm that consists of two major steps: the motion update step and the measurement update step. Two principal solutions of the SLAM problem are the EKF SLAM and the Fast SLAM algorithms. Their general overview with a description of the main steps of these algorithms and problems associated with them are given in the next sub-section SLAM solutions Currently, there exist two major well-known solutions of the SLAM problem: Extended Kalman Filter based SLAM (EKF SLAM), and Fast SLAM that is based on the Particle Filter [8]. In the following subsections, descriptions of both algorithms are provided together with some discussions on the problems associated with them Extended Kalman Filter SLAM Before giving the description of the EKF SLAM algorithm, some preliminaries are provided on the Kalman and the Extended Kalman Filters to recall the main idea and formulas behind them. The Kalman Filter and the Extended Kalman Filters are the main representatives of the most popular Gaussian filters family. The main difference between these two filters is the systems they can be applied to. The Kalman Filter deals with linear systems whereas the Extended Kalman Filter is a direct extension of the Kalman Filter to the non-linear case. Gaussian filters satisfy some constraints: 1) the belief state that reflects the agent s knowledge about the environment (in this case, the positions of landmarks and the agent s pose) is represented by a Gaussian distribution ( ), 2) the noise entering the system is assumed to be a zero mean Gaussian distribution. These constraints are main limitations of the Gaussian filters family. Despite these limitations, Gaussian filters family is still the most popular one. 14 ( )

15 Where a mean vector and a covariance. If the dimensions of the state vector (x) is n-by-1 than the dimensions of is n-by-1 and is n-by-n quadratic symmetric and positive-semidefinite matrix. In our case, a mean vector consists of 3+2*Nl components (Nl total number of landmarks). The first three components correspond to the estimated agent s pose (x,y-coordinates, -orientation), remaining components correspond to the estimated positions of landmarks (x,ycoordinates). Covariance matrix characterizes the correlation between the components of the state vector. KALMAN FILTER Kalman Filter is used to deal with linear systems of the form: ( ) Where state vectors at time moments k and k-1, a control vector applied to a system at a time moment k-1; a measurement vector; matrices of a motion and a measurement models;, random variables of a zero mean noise with covariance matrices and respectively. The Kalman Filter takes as inputs the previously estimated mean vector and covariance matrix ( ), the control input applied at time step k-1 ( ), and the measurements taken at time moment k ( ). Outputs of the filter are new estimated mean vector and covariance matrix ( ). The Kalman Filter algorithm is subdivided into two main steps: the time/motion update step and the measurement update step. Standard formulas of the algorithm are reported here for the convenience of the reader: TIME UPDATE STEP: MEASUREMENT UPDATE STEP: 15 ( ) ( )

16 ( ) ( ) ( ) Where a Kalman gain that shows how much we rely on measurements, I an identity matrix of a size equal to dimensions of a system s measurement vector. EXTENDED KALMAN FILTER In practice, systems are often represented by nonlinear functions that assume in the general case the following form: ( ) Therefore, in order to apply formulas of the Kalman Filter algorithm in the nonlinear case, there is a need for linearization of nonlinear functions, which is commonly achieved via Taylor linearization. Formulas for the Extended Kalman Filter algorithm are given below: TIME UPDATE STEP: MEASUREMENT UPDATE STEP: Where ( ) ( ) ( ) ( ) ( ) ( ) ( ) 16

17 EKF SLAM The EKF SLAM solution is a well-known solution of the SLAM problem. As it is based on the EKF algorithm described previously, it makes the same assumption of the noise and position distributions (a Gaussian zero mean distribution). A feature-based map is used to represent the environment. This map consists of a set of point landmarks with their corresponding positions and signatures. Signatures are landmarks unique ID numbers that enable to distinguish one landmark from another. The data association mechanism deals with the problem of assigning a unique ID number to each landmark. It is applied in the situations when landmarks cannot be uniquely identified. The problem of the data association is beyond the scope of this thesis and for simplification it is assumed that each landmark has its ID number different from others. Letting aside the data association problem, the EKF SLAM algorithm consists of two main steps the motion update step and the measurement update step. Schematically, the EKF SLAM algorithm can be represented as in Figure 1. MOTION UPDATE MEASUREMENT UPDATE Figure 1 The EKF SLAM algorithm In the EKF SLAM algorithm, a mean vector ( ) consists of an agent s pose components (a position and an orientation). A covariance matrix ( ) is fully correlated: all its components are linked to each other. From the equations given above, key advantages and disadvantages of the EKF SLAM algorithm can be noticed. The positive side of the EKF SLAM algorithm is that it frequently leads to an accurate resultant map. This benefit is due to its fully correlated 17

18 covariance matrix: when an agent closes the loop, its own pose and the location estimation of each previously detected landmark are improved. This event is termed the loopclosure. At the same time, this advantageous feature of the EKF SLAM algorithm leads to its main disadvantage high computational cost. As it can be seen in the EKF SLAM algorithm shown in Figure 1, in the measurement update step a mean vector and a covariance matrix are updated each time new observations are taken. Consequently, as the number of detected landmarks grows, the number of computations grows quadratically. As a result, this SLAM algorithm cannot be applied in large environments with a big number of landmarks. The other negative side of the EKF SLAM algorithm is its assumptions of a Gaussian pose distribution and a zero mean Gaussian noise distribution Fast SLAM The second key solution of the SLAM problem is based on the Particle Filler and referred as the Fast SLAM. This sub-section starts by a brief presentation of the general idea behind the Particle Filter. Then, some information concerning the Fast SLAM itself is provided. PARTICLE FILTER The Particle Filter is a non-parametric unlike the Kalman Filter algorithm. The main difference of this filter is that a belief is represented as a set of particles. Particles are samples of a posterior distribution. Similarly, to the Kalman Filter, inputs to the Particle Filter at a time instant k are a set of particles that represent previous poses of an agent (, where N is a number of particles), a control vector applied at a time instant k-1 (, and a vector of measurements taken at a time instant k ( ). The output is an updated set of particles (,). The Particle Filter comprises three main steps: 1 st step for each particle, a new pose is computed based on the previous pose and the applied control input, 2 nd step particles are weighted according to the taken measurements: the higher the probability of receiving these measurements given a particle pose, the higher the value of the weight assigned to a particle; 18

19 3 rd step resampling is performed: particles are selected and added to the updated set with a probability proportional to its associated weight. It results in the elimination of particles with low weight and in the addition of particles equal to those with the high value of the weight. Since the inference process in the Particle Filter relies on sampling methods, general probability distributions can be used. In this way, the main limitation of the Kalman Filter that all the probabilities are represented as Gaussians is avoided. Another important feature of this filter is that this it can be applied to both linear and non-linear systems. The main parameters of the Particle Filter that have to be considered before applying it are how many particles to use and how often to perform the resampling. FAST SLAM As in case of the standard Particle Filter, in the Fast SLAM algorithm particles represent an agent s pose. Each particle maintains its own map and consists of a current and all previous poses of an agent, it includes also all detected landmarks that are represented by their estimated mean locations and associated covariance matrices. Main steps of the basic Fast SLAM algorithm with known correspondence/data association are given below: 1 st step A proposal distribution is computed for each particle based on the applied control vector, 2 nd step Depending on taken measurements, a map of each particle is updated. If a new landmark is observed, its mean position and its covariance matrix are computed. If a previously seen landmark is detected, the standard Extended Kalman Filter is used to perform a map update. All the previously detected landmarks that are not observed this time remain unchanged. 3 rd step The importance weight is computed. If only new landmarks are detected, the assigned weight is set to 1/N, where N is a total number of particles. Otherwise, the weight that is inversely proportional to the difference between obtained and expected measurements is computed. The bigger the difference between measurements, the lower the probability that this particle survives after the resampling phase. 4 th step Resampling: the higher the value of the weight associated with a given particle, the higher the probability it will be selected for the next step. Resampling is performed N-times to ensure the constant number of particles at each time step, as a result particles with big weight are selected several times, while particles with low weight tend to be eliminated. 19

20 Computational complexity and data association This section briefly discusses the main problems associated with the described SLAM solutions and presents currently used ways to manage them. According to the state of the art, major issues of SLAM are data association and computation complexity [9]. Data association attempts to associate detected landmarks with the ones held in the map. There exist several ways to deal with this problem. Some of them are introduced below. According to the Statistical Validation technique, a detected landmark is added to a map only if the distance between this landmark and all others held in the map exceeds a predefined threshold. In case an agent s pose is very uncertain, this technique fails. Batch Validation technique exploits the following idea: first, detected landmarks are placed in a provisional list. If a landmark from this list is not observed when it is expected several times, it is removed from the list, otherwise, it is added to a map. Appearance Signatures technique uses different sensors, for example, a camera and a laser sensor, to detect similarities between landmarks. Generally, the application of this technique makes the SLAM algorithm more robust to the erroneous data association, but at the same time augments the computation cost. Several techniques are proposed to diminish the computation cost. State Augmentation technique is used to reduce computations in the motion update step while ensuring the same performance. This is achieved by re-formulating the motion update step to avoid unnecessary computations. As only an agent s pose is affected by a control vector, there is no need to re-compute landmark-to-landmark elements of a covariance matrix that is done in a straightforward SLAM implementation. Partitioned Updates technique reduces computations in the measurement update step. The main idea is to avoid updating the whole covariance matrix each time observations are taken. In the measurement update step, only a small number of landmarks that belong to a small local region is updated. The update of the whole covariance matrix happens less frequently. Submapping technique decreases the computation cost of the measurement up-date step by dividing a map into smaller sub-maps. Landmarks that are located nearby are estimated with respect to the coordinate frame of a sub-map they belong to, using the 20

21 standard EKF SLAM algorithm. Later, all these sub-maps are combined to create a common map. Sparsification technique achieves better, in terms of computation, performance of the Extended Information Filter SLAM, which is presented in the successive section. This is achieved by exploiting the following feature of the EIF SLAM: almost all elements of an information matrix are close to zero, therefore, setting them to zero transforms an information matrix to a sparse matrix that results, as it is shown later, in the computationally effective SLAM algorithm. The environment where the SLAM algorithm is applied has a crucial importance in the choice of the appropriate SLAM technique. Environment introduces the other issues that have to be taken into considerations. Nowadays, the SLAM problem is solved for small static environments with distinguishable landmarks, but there are still a lot of problems to address for complex and large environments. The focus of this thesis is on the computation complexity issue. According to [9] and based on the analysis of the state of the art, the SLAM solution in the information form has significant unexplored potential for large environments. Therefore, it was decided to analyze a recently proposed quasi-constant Sparse Extended Information Filter SLAM that decreases significantly computational cost Sparse Extended Information Filter SLAM Sparse Extended Information Filter SLAM was recently proposed by Sebastian Thrun et al. [7] as a possible solution of the SLAM problem. However, the potential of SEIF SLAM remains not well explored. This sub-section is devoted to the analysis of SEIF SLAM that is advantageous in terms of computation cost and can be easily extended to Multi-agent SLAM Preliminaries This sub-section starts by providing some preliminaries about the Information Filter and its extension to the non-linear case the Extended Information Filter. 21

22 INFORMATION FILTER The Information Filter is dual to the Kalman Filter and also can be applied only to a linear system. The core distinction between these two algorithms is the way the Gaussian belief is represented. The Kalman Filter uses the moments parameterization (a mean ( ) and a covariance matrix ( )) to represent a belief, while the Information Filter uses so called canonical parameterization (an information vector ( ) and an information matrix ( )) for the same purpose. The information matrix is the inverse of the covariance matrix ( ); the information vector is defined as the product of the information matrix and the mean vector ( ). ( ) ( ) The derivation of the canonical parameterization can be obtained from the standard formula of the multivariable normal distribution ( ), by a multiplication of exponent s components. The procedure of the derivation is shown in Figure 2: Figure 2 Derivation of the canonical parameterization The Information Filter algorithm takes as inputs the previously estimated information vector and information matrix ( ), a control vector applied at a time step k-1 ( ), and measurements taken at a time instant k ( ). Outputs of the filter are the new estimated information vector and information matrix ( ). The Information Filter algorithm, similarly to the Kalman Filter algorithm, consists of two main steps: the time/motion update step and the measurement update step. Formulas used in these two steps are given below. TIME UPDATE STEP: 22 ( ) ( )

23 MEASUREMENT UPDATE STEP: ( ) ( ) EXTENDED INFORMATION FILTER The extension of the Information Filter to the non-linear case via Taylor linearization leads to the following algorithm: TIME UPDATE STEP: MEASUREMENT UPDATE STEP: ( ) ( ) ( ) ( ) ( ) ( ) The difficulty that makes the Extended Information Filter more computationally expensive is a need for a mean vector recovery at each algorithm cycle. A mean vector is essential to implement linearization that cannot be avoided. Recovering of a mean vector requires an inversion of an information matrix that significantly increases the computational cost. This is one of the main reasons why the Extended Information Filter is less popular than the Extended Kalman Filter. However, there are some approaches that deal with the described problem. In the next subsection, a possible way of decreasing the computational cost of a mean recovery procedure is discussed. Let us now enumerate the positive sides of the Extended Information Filter. First, in case of global uncertainty, it is easier to work with the information matrix, where all the components are simply set to zero ( ), than with a covariance matrix of the infinite magnitude for the Extended Kalman Filter. Second, as it can be seen from formulas ( ) and ( ), measurement updates are additive and, therefore, do not require a lot of computations. This feature of the Extended Information Filter makes it is beneficial for systems involving a great number of variables. The third advantage that the canonical parameterization provides is its natural adaptation for a multi-agent case. This is due to the 23

24 fact that the integration of the information collected by several agents is a simple addition operation. All the advantages of the Extended Information Filter mentioned above demonstrate its potential. However, additional improvements are necessary to make it an efficient SLAM solution. The primary disadvantage of the Extended Information Filter is extra computations due to a mean vector recovery that requires an inversion of an information matrix. The Sparse Extended Information Filter SLAM recently proposed by Sebastian Thrun [7] overcomes the problem of high computation cost by introducing the sparsification step that leads to an efficient quasi-constant time SLAM solution with an approximate mean recovery technique. The following sub-section provides the detailed description of the SEIF SLAM Description of Sparse Extended Information Filter SLAM The SEIF SLAM algorithm is a quasi-constant SLAM algorithm that is based on the Extended Information Filter described in Section The key features of this algorithm that make it computationally efficient comes from the introduction of the sparsification step and the use of an approximate technique for a mean vector recovery. Before going into detailed mathematical description of each step, some information and an introduction on the terminology used in the SEIF SLAM algorithm are provided. The majority of the off-diagonal components of an information matrix of the standard Extended Information Filter are very small but none of them is zero, consequently, all agent-landmark links consists of only non-zero components. The farther an agent from a detected landmark the weaker the link between them and the smaller values of corresponding agent-landmark components of an information matrix. The key idea of the SEIF SLAM is to control the number of landmarks with non-zero agent-landmark components that are termed active landmarks by defining a-priori the desired number of these landmarks. This control is achieved by the introduction of the sparsification step. This step limits a number of active landmarks at each time step. If there are more active landmarks than the predefined number, active landmarks with greater time distance to an agent (the earliest detected) are deactivated by setting agent-landmark components to zero. Clearly, the information matrix obtained is an approximation of the true information matrix. This approximation results in less accurate map but leads to a quasi-constant SLAM algorithm that can be applied online. 24

25 The example of the SEIF SLAM information matrix with 3 active landmarks and the EIF SLAM information matrix are given in Figure 3. As it can be seen, the resultant information matrix obtained by the EIF SLAM is fully correlated, while the one obtained by the SEIF SLAM mostly consists of zero components. The number of landmarks used for this simulation was 25. The total number of components of the resultant information matrix is (3+2*25) 2 = As is it shown in Figure, the total number of the non-zero elements of the SEIF SLAM information matrix is 817 out of The right upper blue area of the SEIF SLAM information matrix shows the landmarks that are currently active nz = 817 The SEIF SLAM information matrix nz = 2809 The EIF SLAM information matrix Figure 3 Comparison of information matrices The following assumptions have been made in this thesis: - only the 2d case is considered, - data association problem is neglected by assigning to each landmark an unique ID number; - landmarks used for the simulation are point landmarks that represent distinguishable features (e.g. rocks) on a planet's surface; - obstacle avoidance mechanism is not considered. The cycle of the SEIF SLAM algorithm is composed of four main steps: the motion update step, the measurement update step, the sparsification step, and the approximate mean recover step. The sequence of steps is shown in Figure 4. 25

26 MOTION UPATE MEASUREMENT UPDATE SPARSIFICATION MEAN RECOVERY Figure 4 The SEIF SLAM algorithm MOTION UPDATE STEP In the motion update step, an information matrix, an information vector, and an estimated mean vector are updated based on applied control inputs. A control vector ( ) applied to an agent at a time instant k consists of two components: a linear velocity ( ) and an angular velocity ( ). A differential or a car-like mobile robot is an example of a commercial robot that is actuated by applying control inputs defined by a linear and an angular velocities. The motion model used for the simulation is based on the following formula: ( ) Where sampling time. components of the agent s pose at the time instant k-1, All formulas used in the motion update step are given in Figure 5. The detailed derivation of all the formulas can be found in [10]. In this algorithm, comes from Odometry equations given above, is a partial derivative of the ( ), is covariance matrix of the motion model noise, and is a projection matrix of the form: 26

27 ( ) Where N total number of detected landmarks. ( ) MOTION UPDATE STEP Figure 5 Motion Update Step (SEIF SLAM) In the motion update step, agent-agent components and all components associated with active landmarks are updated. The sparsification step that is responsible for ensuring limited number of active landmarks at each cycle leads to the quasi-constant computational complexity of the motion update step. The result of the motion update step introduces following modifications. First, given a previous pose of an agent and applied control inputs, a predicted mean vector is computed, where only components of a mean vector corresponding to an agent s pose are updated. Second, the information matrix is changed in several ways: components of the matrix that link an agent s pose and estimated positions of active landmarks are weakened (corresponding values are decreased), this is due to the presence of noise in applied control inputs. The information about an agent s pose is partly lost and, as a result, there is 27

28 less confidence in an agent s pose with respect to any landmark s position. At the same time, links between active landmarks are strengthened because the information about their relative positions are not lost. In order to demonstrate the effect of the motion update step, the simulation of the SEIF SLAM algorithm was performed. Information concerning the graphical representation used in showing the simulation results is given in Table 1. The simulated environment is shown in Figure 6. Graphical representation true landmark s position Table 1 Graphical representation Explanation uncertainty of landmark s position true agent s position predicted agent s position agent s position after the measurement update predicted agent s position uncertainty agent s position uncertainty after the measurement update step agent s position uncertainty after the sparsification step Figure 6 Simulation of the SEIF SLAM algorithm (the motion update step) 28

29 An agent with the initial position (0,0) starts navigating in circular trajectory. As it moves its uncertainty, shown as a blue ellipsoid around the agent, increases. On the way, it detects three landmarks, the true locations of which are represented by red stars. How much the confidence of the agent pose is decreased is defined by the level of noise in the motion model. The effect of the motion update step on the information matrix is shown in Figure 7. Figure 7 Effect of the Motion Update Step on the Information Matrix As it can be seen, the certainty of the agent s pose (first 3-by-3 green sub-matrix on diagonal) is decreased as well as the one of the agent-landmark components of the matrix. Meanwhile, the certainty of landmark-landmark components that are shown in blue color is increased. 29

30 MEASUREMENT UPDATE STEP In the measurement update step, the following modifications are done: as an agent detects a new landmark, it is added to a map and agent-landmark components of the Information Matrix that corresponds to the detected landmark are updated. Additionally, agent-agent and landmark-landmark sub-matrices of the Information matrix are updated. It is also important to mention that how strong is this new link is defined by the level of noise in the measurement model. Measurements are received from the range-bearing sensor that measures the distance between the current position of an agent and a detected landmark ( ) and provides the information about the orientation of a landmark s position with respect to the agent s current frame ( ). Consequently, the measurement vector ( ) consists of a distance, an angle, and a unique ID number of a landmark. The sensor can be defined by setting the following parameters: the sensing range and the sampling time. Based on the information provided by the sensor, coordinates of estimated position of the i-th landmark can be computed: ( ) Where predicted pose of the agent. The measurement model used for the simulation is given by the following equations: ( ) Where, differences of x and y coordinates, respectively. All formulas used in the measurement update step are provided in Figure 8, where the expected vector of measurements. is 30

31 MEASUREMENT UPDATE STEP for all detected landmarks do end Figure 8 Measurement Update Step (SEIF SLAM) In the measurement update step, components that are on the main diagonal, the agentagent and agent-landmark components of detected landmarks are updated. Local additions that take place in the measurement update step are independent from dimensions of the information matrix, the complexity of this step depends only on the number of measurements returned by the sensor. In order to demonstrate the result of the measurement update step, a simulation similar to the one described in the motion update step part was performed. The moment when an agent detects the third landmark is shown in Figure 9. Figure 9 Simulation of the SEIF SLAM algorithm (detection of the new landmark) 31

32 The effect of the measurement update step on the information matrix is presented in Figure 10. It can be seen that the measurement update step modified only the agentlandmark components of the information matrix that correspond to the detected landmark. The agent-landmark components are shown in yellow color. This time, only one landmark (N 3) was detected. As a result, the landmark N 3 becomes active. Also, agent-agent (upper green 3x3 matrix ) and landmark-landmark (green 2x2 matrix) components of the observed landmark are up-dated. This result is explained by the received measurement vector: as no information was gained concerning previously seen landmarks (N 1 and N 2), components associated with them are left unchanged. We can also mention that once a new landmark is added to a map, it is linked only with the agent. In our case, the new landmark N 3 was added to the map and as we can see it is not linked with landmarks N 1 and N 2. Figure 10 Effect of the Measurement Update on the Information Matrix 32

33 SPARSIFICATION STEP The sparsification is the key step of the SEIF SLAM algorithm. The purpose of this step is to control the number active landmarks that leads to the quasi-constant on-line SLAM algorithm. Let us now explore the mechanism that is behind this step without going deeply into formulas. In order to show the effect of the sparsification step, the simulation of the SEIF SLAM was performed. The number of active landmarks was set to 3. In Figure 11, the moment when the agent detects the 4 th landmark is shown. The black ellipsoid around the agent shows the uncertainty of the landmark s position after the sparsification was performed. In this case, the effect of the sparsification on the agent s pose uncertainty is negligibly small. However, it is well known that the sparsification procedure introduces overconfidence and the resultant information matrix is an approximation of the initial one. How much the sparsified information matrix is different from the original one depends on the magnitude of components that are set to zero. Later on, the phenomenon of the overconfidence will be discussed in more details. The data that demonstrates its effect on the agent s pose uncertainty is provided in the next section. Based on the state of the art, a way to overcome this problem is discussed Figure 11 Simulation of the SEIF SLAM algorithm (the sparsification step) 33

34 The effect of the sparsification step on the information matrix is shown in Figure 12. In this case, the first seen landmark is sparsified or, in other words, the agent-landmark components associated with the first landmark are set to zero. By analyzing the components of the information matrix given in this figure, the effect of the distance on how strong the links are can be seen. It can be noticed that the agent-landmark components of the first landmark linking the agent and this landmark are the smallest ones compared to the corresponding components of the other landmarks. As it is discussed in the motion update step part, the bigger the time distance between an agent and a landmark, the weaker the link that connects them. Here, the effect of this property is clearly shown. It can be also seen that the closer two landmarks to each other, the stronger the link between them is. It is important to notice that by setting the discussed sub-matrices to zero, the information is not completely lost. Part of this information is distributed into neighboring components of active landmarks. The sparsification leads to the sparse information matrix gives the possibility of up-dating the filter in efficient way, of course, at a price of the information loss. Figure 12 Effect of the Sparsification Step on the Information Matrix 34

35 All formulas used in this step are given in Figure 13. Using notation from [10], is a list of landmarks that have to be deactivated, is a list of the landmarks that have to remain active after the sparsification step. are projection matrices similar to the one used in the motion update step. SPARSIFICATION STEP Figure 13 Sparsification Step (SEIF SLAM) MEAN RECOVERY STEP The final step of the SEIF SLAM algorithm is the mean recovery step. As previously noted, one of the main reasons why the Extended Kaman Filter is more widely used than the Extended Information Filter is the need of the latter to perform the mean recovery operation. The recovery procedure is necessary for Taylor linearization of the system and it requires the inversion of the information matrix that is a costly operation, especially if the system includes a great number of state variables, as it is the case with the SLAM problem. Therefore, in order to guarantee the applicability of the SEIF SLAM algorithm for large environments with a big number of landmarks this problem has to be overcome. In this thesis, the approximate algorithm for the mean recovery that avoids matrix inversion proposed in [10] is employed. A mean vector, in 2D case, consists of x,y coordinates of the agent s position, the agent s orientation (, yaw), and x,y coordinates of detected landmarks. At each cycle, one needs to know only values of mean vector components associated with the agent s pose and active landmarks. The approximate mean recovery strategy is obtained by re-formulating the mean recovery problem as the following optimization problem that comes from the Gaussian distribution formula in canonical parameterization form: ( ) 35

36 The expression in brackets is re-written in terms of each component of the mean vector ( ). Next, the derivative of this expression is taken with respect to the component that has to be recovered and the obtained expression is set to zero. In this way, the procedure of the mean recovery is performed. The final formula used in the implemented SEIF SLAM algorithm is given below: Where a projection matrix that for the landmark i has the following form: ( ) ( ) Where total number of landmarks Simulation results In this section, the results of performed in MATLAB simulations are provided together with their description and analysis Implementation of EKF SLAM and EIF SLAM Extended Kalman Filter SLAM and Extended Information Filter SLAM are identical from the analytical point of view. Consequently, with the same initial conditions and applied control inputs both algorithms have to lead to equal result. Two SLAM algorithms for a single agent were implemented in the MATLAB environment to verify the correctness of the program code and in order to demonstrate some properties of these SLAM algorithms that were discussed previously. DESCRIPTION OF SIMULATION The environment consists of ten landmarks. An agent starts from the initial position (0,0) with the initial orientation. The agent follows the circular trajectory 36

37 defined by the constant control vector. The sensing range is set to 3, the sampling time. The covariance matrix of the zero-mean noise introduced by noisy control inputs is. The covariance matrix of the zeromean noise introduced by measurements is. The EIF SLAM and the EKF SLAM algorithms showed equal behavior in the loopclosure simulation. The results of the EKF SLAM algorithm implementation are given below. EFFECT OF LOOP-CLOSURE As the agent navigates, its pose uncertainty grows proportionally to the noise introduced by the control vector. The uncertainty of a landmark s position depends not only on the uncertainty of the agent pose at the moment when it was detected for the first time, but also on the noise in the measurement vector. The effect of the continuous increase of the uncertainty is shown in Figure 14. Figure 14 Effect of the noise in the motion and the measurement models Now, let us take a look at the phenomenon known as the loop-closure that takes place when the agent detects the landmark with smaller covariance, in our case, the first landmark that was observed. The impact of the loop-closure phenomenon on landmarks uncertainties is shown in Figure 15. Two plots show the state of the map before and after the loop closure. 37

38 Figure 15 Effect of the loop closure on the uncertainties of landmarks Before the agent detects the landmark with high position confidence, its pose uncertainty is low. When the agent detects the landmark with smaller pose uncertainty, it reduces its pose uncertainty by self-relocalizing with respect to this landmark. It can be seen in Figure 16. Agent s pose uncertainty before closing the loop is shown as blue ellipsoid. Its uncertainty after closing the loop is small green ellipsoid. The significant increase of the agent s pose confidence is observed Figure 16 Effect of loop-closure on the agent s pose uncertainty Clearly, the measurement update step corrects also the estimated position and the orientation of the agent. In order to demonstrate this effect, Figure 16 is zoomed in and it is shown in Figure 17. Small blue circle represents the agent s estimated position before the loop closure, violet circle is the corrected agent s position, and small red star is the true 38

39 agent s position. As it can be noticed, the corrected agent s position is closer to its true position Figure 17 Correction of the agent s pose ( loop-closure ) Since the agent is linked to all previously seen landmarks through the covariance/information matrix, their positions are corrected and their position uncertainties are reduces as well. As a result, the increase in the map confidence is observed. EIF SLAM vs. EKF SLAM: COMPUTATIONAL COST The Extended Information Filter is dual to the Extended Kalman Filter. The main difference is the parameterization used to represent the belief. Clearly, this distinction leads to variations in the formulas used in the motion update and in the measurement update steps. As it can be seen from formulas given previously, the motion update step is additive and, as a result, is not very costly in case of EKF ( )-( ), while, in case of EIF, this step requires the inversion of two matrices that makes this step costly ( )-( ). The situation is reversed in case of the measurement update step (EKF: ( )-( ), EIF: ( )-( )). The comparison of the analyzed SLAM algorithms, in terms of computational cost, is shown in Table 2. The result validates the expected behavior. Table 2 Comparison of EKF SLAM and EIF SLAM in terms of computations SLAM algorithm Average elapsed time of the motion update step, [s] Average elapsed time of the measurement update step, [s] EKF SLAM EIF SLAM

40 AVERAGE MEAN ERRORS Several simulations were done, where the average error of the agent s pose was obtained, with the aim of comparing the performance of examined SLAM algorithms. These simulations are subdivided into two parts. In the first part, the effect of the covariance matrix of the motion model on the average pose error for both algorithms were analyzed. In the second part, the influence of the sensing range was analyzed. Each algorithm has been run 500 times to obtain the average error. The following covariance matrix of the measurement model was used: COVARIANCE MATRIX OF THE MOTION MODEL Average errors were calculated with different values assigned to the covariance matrix of the motion model. In the first part of simulations, the sensing range was set to 2. The results of simulations are given in Table 3. The obtained results show that as the values of the covariance matrix grow, the average error on the agent s position and orientation grows concurrently. Both SLAM algorithms demonstrate alike behavior. Clearly, the system is subjected to bigger noise with covariance matrix that consists of components with greater values. At each step, the x,y coordinates are changed by 1 and the orientation ( ) is changed by 0.1. The last test of this part was done with big values of the covariance matrix that corresponds to the error equal to 50% of the total agent s movement in the components of the agent s pose. As the data shows, the system diverges in both SLAM algorithms. Table 3 Average error N 1 Covariance of the motion model Average error SLAM algorithm -coordinate - coordinate, [rad] EKF SLAM EIF SLAM EKF SLAM EIF SLAM EKF SLAM EIF SLAM

41 SENSING RANGE In this part, the value of the sensing range was varied in order to demonstrate its impact on the average error. The following covariance matrix of the motion model was used :. Clearly, a big sensing range provides more information about the environment, as it covers larger area. Each time an agent detects a landmark, it relocalizes itself with respect to this landmark. With a big range, an agent detects more landmarks at each step and, therefore, it relocalizes itself more often, which leads to a smaller pose error. In other words, a big sensing range results in small average errors. Consequently, the behavior shown in Table 4 is observed. It can be mentioned here that the more there are landmarks in the environment, the better the agent s pose estimation is, but, at the same time, this leads to the complication of the data association problem, as landmarks are close and the probability of false data association is higher. Table 4 Average error N 2 SLAM algorithm Sensing Range Average error -coordinate - coordinate, [rad] EKF SLAM EIF SLAM EKF SLAM EIF SLAM EKF SLAM EIF SLAM EKF SLAM EIF SLAM In all simulations discussed in this sub-section the EIF SLAM and EKF SLAM algorithms showed alike behavior. The results obtained correspond to the expected ones. Therefore, it can be concluded that both filters are implemented correctly. From now on, on the basis of the implemented EIF SLAM algorithm, the interest is switched to its extension to the SEIF SLAM algorithm by introducing the sparsification and the approximated mean recovery steps. 41

42 Implementation of SEIF SLAM In this part of the thesis, simulation results of the implemented SEIF SLAM algorithm are provided. First, the focus is on the analysis of the variation of the average pose error depending on the number of active landmarks. Then, it is switched to the effect of the introduction of the approximate mean recovery step. Next, results of the SEIF SLAM algorithm simulation are given and compared to those of the EIF SLAM algorithm. In the end, the loop-closure is simulated and results obtained by the EIF SLAM and SEIF SLAM algorithms are compared. ANALYSIS OF THE SPARSIFICATION STEP The sparsification step transforms the original information matrix and the original information vector into approximated ones, and this influences the accuracy of the algorithm. Clearly, the bigger the number of active landmarks the smaller the degree of the approximation and, as a result, the more precise is the resultant map of the environment. Some simulations were performed in order to demonstrate the effect of different numbers of active landmarks on the precision of the resultant map. The results of simulations are given in Table 5. Table 5 Effect of the sparsification step on the average pose error SLAM Number of active Average error Algorithm landmarks -coordinate - coordinate, [rad] EIF SLAM 10/ EIF SLAM with the sparsification 1/ step EIF SLAM with the sparsification 2/ step EIF SLAM with the sparsification 3/ step EIF SLAM with the sparsification step 4/

43 The obtained data demonstrates the expected behavior. As we can see, as the number of active landmarks grows, the average error of the agent s pose decreases. In this case, 4 active landmarks lead to a good approximation. ANALYSIS OF THE APPROXIMATE MEAN RECOVERY STEP In this part, the comparison of EIF SLAM and EIF SLAM with the approximate mean recovery step algorithms is made in terms of its influence on the accuracy of the final map. The result of applying EIF SLAM and EIF SLAM with the approximate mean recovery are given in Table 6. Two simulations with different values of the covariance matrix of the motion model were done. As it is expected, the more precise the applied control law, the smaller the observed error is. In both case, the effect of introducing the approximate mean recovery step is visible, in the first case the difference of resultant error is small and can be neglected while in the second case the effect of approximation is stronger. For the future work it is recommended to analyze other techniques used for the mean recovery to increase the precision of the obtained map. The results of this test underline the importance of an accurate motion model that is defined by the exploited Odometry technique and by the precision of sensors. Table 6 Effect of the approximate mean recovery step on the average pose error SLAM Covariance matrix of Average error algorithm the motion model -coordinate - coordinate, [rad] EIF SLAM EIF SLAM with the approximate mean recovery EIF SLAM EIF SLAM with the approximate mean recovery

44 RESULTS OF EIF SLAM AND SEIF SLAM SIMULATION In this part, the comparison of the EIF SLAM and the SEIF SLAM algorithms are provided. Algorithms were compared in terms of the resultant average agent s pose error depending on a total number of landmarks. The results of this test is shown in Table 7, maps used for these simulation are given in Table 8. In all the performed simulations, the number of active landmarks was set to four. SLAM algorithm EIF SLAM SEIF SLAM ( ) EIF SLAM SEIF SLAM ( ) EIF SLAM SEIF SLAM ( ) EIF SLAM SEIF SLAM ( ) EIF SLAM SEIF SLAM ( ) Table 7 Comparison of the EIF SLAM and the SEIF SLAM algorithms Average error Number of Total - -, landmarks error coordinate coordinate [rad] Difference % % 25.4% 36.6% 49.77% As it can be noticed from the data given in Table 7, as the number of landmarks grows both the average and total pose errors decrease. This happens due to better agent s localization with respect to detected landmarks. At the same time, we can see that the more landmarks we have, the bigger the percentage of the total error introduced by the sparsification and by the approximate mean recovery steps compared to the one of the EIF SLAM algorithm. 44

45 Table 8 Maps used for simulations Number of landmarks = 10 Number of landmarks = 26 Number of landmarks = 50 Number of landmarks = 100 (simulation N 1) Number of landmarks = 100 (simulation N 2) EIF SLAM vs. SEIF SLAM: COMPUTATIONAL COST As it was already discussed in Section 1.3.2, the key advantage of SEIF SLAM compared to EKF SLAM (and, clearly, to EIF SLAM as they are dual to each other) is its quasi-constant computational cost. Therefore, it would be interesting to take a look at this feature in practice. With the aim of comparing the performance of the SEIF SLAM algorithm with the one of EIF SLAM, several simulations were done. In this simulation, an agent starts from the position (0,0) and with the initial orientation 45 deg. It follows the straight-line trajectory defined by the constant linear velocity. As it moves, it observes landmarks that are evenly spaced at 1 m distance between each other. The sensing range was set to 3 [m]. The number of active landmarks was 3. Covariance matrices of the measurement noise and the motion noise are the following: and. 45

46 CPU time, [s] CPU time, [s] CPU time, [s] CPU time, [s] The Motion Update Step COMPUTATIONAL COST The Measurement Update Step The SEIF SLAM The EIF SLAM 0.15 The SEIF SLAM The EIF SLAM Number of landmarks The Mean Recovery Step Number of landmarks The SEIF SLAM vs. the EIF SLAM 0.15 The SEIF SLAM The EIF SLAM The SEIF SLAM The EIF SLAM Number of landmarks Number of landmarks Figure 18 CPU time The performance of the SEIF SLAM and the EIF SLAM algorithms were evaluated, based on the required CPU time, for environments with different number of landmarks. The results of this analysis are given in Figure 18. As it can be seen, SEIF SLAM requires almost constant computational time, while the number of computations of the EIF SLAM algorithm grows as the number of landmarks grows. LOOP-CLOSURE In order to demonstrate the behavior of the SEIF SLAM algorithm, the loop-closure was simulated and compared to the result obtained by running the EIF SLAM algorithm with equal initial conditions and the same control law. The number of active landmarks was set to 2. The results of this simulation is given in Table 9. 46

47 Table 9 Comparison of coop-closure EIF SLAM SEIF SLAM ( ) Before the loop-closure After the loop-closure Zoom in The first row shows the map state before the loop-closure, while the second one demonstrates the map after this event. The last row shows the result of the agent s pose correction: a blue small circle is the predicted agent s position, a violet one is a corrected position, a small red star is a true agent s position. As it can be seen, the process of closing the loop in both cases leads to the correction of the agent s mean pose and to the increase of the certainties of landmarks positions and of the agent s pose. 47

48 Overconfidence of SEIF SLAM The phenomenon of overconfidence introduced by the sparsification is a known problem that is caused by the approximation of the original map. To show this phenomenon, the SEIF SLAM algorithm with the number of active landmarks equal to 2 was applied to an agent that follows a circular trajectory. The effect of the sparsification in this simulated test is given in Table 10. Table 10 Overconfidence of the SEIF SLAM algorithm The result of the sparsification Zoom in

49 In the left column, the current position of the agent is shown, in the right column, a zoomed in area around a landmark is given. A green ellipsoid represents a landmark s uncertainty after the measurement update step, a black one is a landmark s uncertainty after the sparsification step. In most cases, the approximation introduced by the sparsification step is negligibly small as it is shown in the first raw, where the green ellipsoid is completely covered by the black one. In pictures that are in the second raw, we can notice a small difference between the green and the black ellipsoids. The strongest effect of the overconfidence that was observed in this simulation is shown in the last raw. This situation took place when the agent has detected two new landmarks at the same time and, as the number of active landmarks was set to 2, the sparsification step deactivated two previously detected landmarks that are still in the agent s sensing range. There are several works done to overcome this difficulty. One of them is the Exactly Sparse Information Filter (ESEIF) SLAM algorithm [11] that deals with the overconfidence introduced by the sparsification, while keeping one of the main qualities of SEIF SLAM of being a quasi-constant algorithm. The main difference is that, in case the agent detects more landmarks than the predefined number of active landmarks, the algorithm subdivides all observed features into two sets. One of them is used in the measurement update step to update the map and the second set of features is used to relocalize the agent. The resultant map is stated to be nearly identical to the one obtained by the EKF SLAM algorithm. Therefore, in order to increase the accuracy on the resultant map, it is recommended for the future work to implement ESEIF SLAM and check its advantages stated in the paper Extension to Multi-agent SEIF SLAM As it was discussed previously, the EIF SLAM algorithm is advantageous in case of a multi-agent system. This benefit comes from the algorithm s natural extension to the multicase due to its canonical parameterization (Section 1.3.1). In order to integrate the data accumulated by several agents and, as a result, to create a common map, it is sufficient to sum corresponding components of information matrices and information vectors. Under the assumption of know correspondence or as we called it before known data association, the operation of the maps integration is a simple additional process that does not require many computations. In the following sub-section, the result of applying the maps integration/collapse technique is provided. 49

50 SIMULATION OF THE MAPS COLLAPSE The following simulation was performed to demonstrate the result of applying the maps collapse technique: two agents were independently exploring the environment, at some point they met and exchanged the collected information. The effect of the maps collapse is shown in Figure 19 Result of the Maps Collapse Figure 19 Result of the Maps Collapse In the upper-left corner, the simulated environment is given at the moment right before the maps collapse technique was applied. On this image, red ellipsoids represent landmarks uncertainties assigned by the first agent, blue ellipsoids are uncertainties assigned by the second one. As we can see, not all the landmarks were detected by both agents. The resultant map is shown in the right-upper corner, where black ellipsoids represent landmarks uncertainties after the maps collapse. On the right and on the leftlower corners two pictures after zooming in are provided. As it can be seen, the maps collapse technique increases significantly the certainty of positions not only of landmarks observed by both agents (the left lower plot) but also landmarks seen by a single agent (the right lower plot). 50

51 Chapter 2 2. ACTIVE SLAM This chapter is devoted to the Active SLAM problem. According to the SLAM classification given in [12], all algorithms, depending on the policy that is used to control agent s actions, are subdivided into two groups: Passive and Active SLAM. In Chapter 1, the attention is focused on Passive SLAM where an agent is purely observing and all the decisions on where to move are made by some other entity. In this chapter, the interest is shifted to Active SLAM. The main problem of Active SLAM is how to navigate an agent in an efficient way that leads to the accurate resultant map in the shortest period. The chapter consists of three sections: Coverage techniques, Active SLAM for a singleagent case, and Active SLAM for a multi-agent case. Section 2.1 discusses coverage techniques for a multi-agent system. Two different situations are considered: a multi-agent system with unlimited communication and a more realistic system that deals with a barrier due to limited communication. All algorithms presented in this section make an assumption of the known agents poses. Section 2.2 focuses on Active SLAM algorithms that take into account imperfection of sensors and unavailability of absolute localization. After the investigation of the state of the art techniques, an appropriate exploration technique is selected and implemented in MATLAB. The simulation results are analyzed and several improvements are proposed to overcome problems that were uncovered during the simulation. Next, an improved algorithm is implemented and tested in MATLAB. In Section 2.3, Active SLAM for a multi-agent case is considered. The extension of the improved Active SLAM algorithm to a multi-agent case is described. Two cases are considered: a multi-agent system with unlimited communication range and a multi-agent system with limited communication range. Algorithms for both considered cases are implemented and the results of performed simulations are provided together with their analysis. 51

52 2.1. Coverage techniques The main goal of a coverage technique is to devise a policy that leads to effective exploration. The main indicator of the efficiency of a coverage technique is the time required to complete the exploration mission. In this section, techniques for multi-agent systems are discussed. The key problem to solve in a multi-agent exploration is where to guide agents to speed up the process of area coverage. Many researches are focused on developing an efficient coverage algorithm, as a result, various techniques have been proposed in the last decades. A frontier-based exploration [1] proposed by Yamauchi is a core of the vast majority of existing techniques. In his paper, he uses a term an occupancy grid map [13] that is the most widespread way of the environment representation. An example of the occupancy grid map representation is given in Figure 20. The environment is divided into equal square cells. Initially, each cell has a uniform occupancy probability that changes during exploration. Each cell can be in one of the following states: unknown if the current probability is equal to the initially assigned one, free if the current probability is lower than the initial probability, and occupied if it is higher than the initial probability. In Figure 20, a blue circle represents an agent exploring the environment, yellow/black cells are explored free/occupied cells, white cells represent unknown cells, and red cells are frontier cells between explored and unexplored area. Figure 20 An example of the occupancy grid map. The principal idea of the frontier-based exploration is to guide an agent to the closest frontier cell, with the goal to obtain new information about the world. However, this approach is not optimal because it does not guarantee that two or several agents would not select the same frontier cell as a target point that leads to a redundant coverage. This approach represents a highly decentralized strategy with minimal coordination. In 52

53 successive sub-sections, other coordination techniques that are based on the described approach are presented Unlimited communication range This sub-section presents several coverage techniques under the assumption of unlimited communication range. Many researchers working in the field of the multi-agent exploration have developed various techniques to overcome the problem of redundant coverage. One of the earliest coordination techniques proposed in [2] overcomes the problem of redundant coverage by making use of the cell utility. This technique is based on the frontier-based approach. The main difference lies in the policy that is used to select a target cell. In the frontier-based approach, the choice of the target cell depends entirely on the cost of travel or, in other words, the distance between an agent and a frontier cell (the bigger the distance, the less attractive is this cell). As it was already mentioned, it leads to redundant exploration. To avoid this situation, a utility value is assigned to each cell. Initially, all cells have equal utility. When a frontier cell is assigned to an agent, the utility of this cell is decreased as well as utilities of nearby cells that are within the agent s sensing range. The decrease of a cell utility is inversely proportional to the distance from the selected frontier cell to the cell under study. [4] proposes a distributed frontier-based algorithm that uses the Particle Swarm Optimization (PSO) [5] to guide agents. A map of the unknown environment is divided into equal sub-areas. Each sub-area is assigned to an agent. Each agent can be in the exploration state or in the walking state. In the exploration state, an agent explores the sub-area it is assigned to, by moving to the closes frontier cells. The A* algorithm is exploited to compute a distance to travel. This algorithm also takes into account the energy consumption aspect. As the rotation of an agent requires a significant use of the energy, frontier cells that can be reached with the minimum rotation angle are considered first. Once the entire sub-area is explored, an agent switches to the walking state. In the walking state, an agent is guided to frontier cells that belong to other sub-areas by the PSO model. The PSO model introduced by James Kennedy' and Russell Eberhart [14] has its roots in the observation of animal behaviours, such as the flocking of birds, the schooling of fishes, etc. The main idea is that the velocity of an agent is updated at each time step and it depends on the local best position and the global best position. The local best position is the closes frontier cell. A frontier cell that is selected as a target cell by a minimum number of agents is the global best position. The choice of whether the local best position or the global best position is 53

54 selected by an agent is made randomly. This policy leads to the distribution of agents and, as a result, the redundant coverage is avoided. The formula used to compute the velocity of i-th agent at a time step k is given bellow: Where the inertia factor that has a value between 0 and 1, the current velocity of i-th agent; acceleration factors; a random number in the interval from 0 to 1; the local and the global best positions respectively; the current position of i-th agent. ( ) How fast an agent decelerates is defined by the value of the inertia factor, while its acceleration depends on values of acceleration factors. From ( ) it can be seen that the farther the agent from the local or the global best positions, the stronger the effect of the 2 nd and the 3 rd components of the given formula and, therefore, the faster it moves. The closer the agent to the target cell, the smaller are these two components and the stronger the effect of the inertia factor becomes. It can be also noticed that when the area is fully explored, the 2 nd and the 3 rd components become zero, and the agent gradually stops. Both techniques described in this sub-section prevent from the redundant coverage problem. However, they are working under the assumption of unlimited or large communication range that provides constant communication between agents Limited communication range This subsection presents recently proposed techniques that address the problem of the limited communication range. There are two general ways of dealing with the limited communication problem. The first way is to keep agents close to each other that leads to longer exploration time. The second one is to allow agents to move out of the communication range, but this can result in overlapping exploration. A possible solution is to allow agents to move temporary 54

55 beyond the communication range and then rendezvous with each other in the location that was previously selected. An algorithm proposed in [4] is a static role-based exploration algorithm. Each agent performs a role of an explorer or a relay. The task of an explorer is to explore the farthest areas of the environment. It periodically returns to previously agreed upon rendezvous points and it communicates what was found to a relay. The task of a relay is to deliver knowledge obtained from an explorer to the command center that collects all the information about the environment. Schematic representation of this algorithm is shown on Figure 21(source: [4]), where A,C are relays and B,D are explorers and green crosses represent the selected rendezvous points. Later on, this static role-based exploration approach has been extended to a dynamic role-base exploration that allows agents to switch previously assigned roles in order to improve the performance [5]. Figure 21 Role-based exploration algorithm Another recently proposed algorithm uses a sector search with rendezvous to coordinate a team of autonomous agents in unknown environments [6]. The principal idea behind this approach is to divide an unknown environment into equal sectors and assign to each agent a particular sector to explore. Periodically, agents meet and communicate their findings to build a common map. The simulation of this exploration technique is shown in Figure 22(source: [6], p. 2403). Figure 22 Sector-search algorithm In this simulation, the environment consists of 4 square obstacles. Three agents are used to explore this environment. Initial positions of agents are close to each other so that 55

56 they can communicate. The environment is divided into three equal segments and each agent is assigned to one of them. The trajectories that have been traversed by agents are shown in red, blue, and green colors. Numbers 1 and 2 show the position where the agents met to communicate what was found. The technique suggested in [15] aims to maximize the information about the environment while minimizing the travelled distance. In this technique, each agent is within a communication range at least with one team member. For this purpose, authors introduce a connection graph, where each node represents an agent and an arc demonstrates its connection with other agents. The target point of each agent is selected among the frontier cells of an agent connected to it based on a travel cost and a cell utility. The example of the agents coordination is shown in Figure 23(adapted from [15]). Figure 23 Coordination of agents Initial positions of agents that explore the environment are represented as black points (A, B, C). As it can be noticed, each agent is within the communication range (Rc) with its neighbor. A* that lies on the border of the agent s sensing range (Rs) is selected as a target point for the 1 st agent based on the expected information gain and the cost of travelling. The target point for the 2 nd agent is selected among the new frontier cells of the 1 st agent when it reaches its target point (A*). The target point of the 3 rd agent is selected in the similar way. The technique proposed in [16] solves the problem of coordination using the potential field based approach. Each agent is attracted to frontier cells and is repulsed from previously detected obstacles/walls as well as from other team members. The typical problem of local minimum is solved by introducing a leader, which has different control law. A leader always navigates, independently from other agents, towards the closest frontier cell. The problem of limited communication is solved by introducing several subteams that have their own leaders. Each leader is within communication range at least 56

57 with one agent from other sub-teams. Fulfillment of this condition guarantees the completion of the exploration task Active SLAM for a single-agent case This section focuses on the Active SLAM problem for a single-agent system. In the beginning, the state of the art of Active SLAM is discussed. In Section 2.2.1, the selected algorithm and its adaptation to SEIF SLAM is described in details. Simulation of this algorithm in MATLAB is given in Section together with the analysis of its performance. In Section 2.2.3, improvements of the implemented algorithm are proposed. In the Section 2.2.4, a modified Active SLAM algorithm is simulated and examined with different parameters. THE STATE OF THE ART The main limitation of techniques discussed in the previous section is the assumption of the availability of absolute localization of agents. Last decades have shown that SLAM is a very powerful tool against uncertainties and noise contained in the sensor data. Hence, the integration of the coverage technique with the SLAM algorithm is the most evident way that would lead to the comprehensive exploration technique. Admittedly, the strategy that is used to guide an agent has a significant influence on the final result. It was proved independently by different authors that the random walk approach does not lead to efficient area exploration due to its inherent redundant coverage. The effective exploration technique under the assumption of known agent pose is defined by the time required to cover the entire area. However, if a system is subjected to noise, the effectiveness, besides timing parameters, is characterized by the accuracy of the resultant map. In case of a planetary exploration mission, accuracy is more important than execution time. Therefore, the policy that determines the decision-making process of where to navigate an agent has to consider both aspects. In the SLAM problem, as an agent moves, its pose uncertainty grows due to the accumulated Odometry error. Clearly, a straightforward application of the frontier-based coverage technique, which constantly navigates an agent towards the most unknown area, would lead to low accuracy of the resultant map. The information gained by an agent with the low pose confidence does not have significant value. A decrease of the agent s pose uncertainty is achieved by re-visiting previously seen landmarks with high position confidence. 57

58 Several techniques that address the problem of the active map building in the presence of uncertainty have been developed. Different ways of dealing with the formulated problem were proposed. All of them take into consideration both the time and the accuracy aspects and they are based on the principle described above. ACTIVE LOOP-CLOSING The Fast SLAM exploration technique is proposed in [17]. This technique is an extension of the frontier-based exploration described in Section 2.1. The policy used to navigate an agent in the frontier-based approach is not applicable for the map-building problem with the presence of noise due to its low-accurate resultant map. In order to overcome this difficulty, the active loop-closing strategy was introduced. The environment is represented by two types of maps: an occupancy grid map and a topological map. The occupancy map representation is used to evaluate the potential information gain that would be obtained by moving to the frontier cell under study. Each particle possesses its own occupancy grid map and topological map. A topological map consists of vertices that represent positions previously visited by corresponding particle and edges that represent the path that this particle has followed to travel from one vertix to another. A new vertex is added to a topological map if the distance between the current position of a particle and all other vertices in its map is bigger than the predefined threshold or if other vertices are not visible from the current particle position. In Figure 24 (source: [17]), the example of such a map representation for the particle s at time step t is given. Red dots and lines represent vertices and associated with them edges, while a green dot represents the particle s current position ( ). is a set of positions of interest that correspond to previously visited locations that are close to the current particle s position but far (long traversed path) based on the topological map. Figure 24 Active loop-closing technique When an agent pose uncertainty becomes too big, it switches to the active loop-closing mode. In this mode, the position of interest from the set that corresponds to particle with the highest value of the weight (the bigger the weight of a particle, the higher the probability that this particle corresponds to the true agent s pose) is selected. This position 58

59 is assigned to the agent as a target point. By revisiting the chosen position, the agent performs the loop-closure and reduces its pose uncertainty in this way. Then, the agent returns to the position it has occupied before switching to the active-loop closing mode and it continues exploration. The described situation is shown on Figure 24(source: [17]). As it can be seen, the set that corresponds to the particle with the highest weight consists of two positions (the left image). The position that leads to better agent s localization (that one that is close to a landmark with good pose confidence) is selected and the agent navigats towards it (the central image). As soon as the agent improves its localization, it returns to the previously occupied location and continues exploration (the right image). MPC AND ATTRACTOR BASED EXPLORATION The active exploration technique proposed in [18] makes use of the attractor implemented together with the Model Predictive Control (MPC) local planning strategy. The MPC is used to select the optimal target point that is within N-step distance from the current agent s position. When the optimal target point is selected, the control vector that moves an agent to the closest cell, which lies on the shortest path to the selected point, is applied. The main limitation of the MPC strategy is that only points that lie within N-step distance are evaluated. In other words, only local area is analyzed. To deal with this limitation, the attractor is introduced. The attractor is the artificially added feature that consists of a mean position and a covariance matrix associated with it. The purpose of the attractor, as its name suggests, is to attract an agent to the point that lies farther that the N- step distance. The attractor is placed within N-step distance to an agent in the direction to the target point that was selected based on the global information about the environment. At each time step, an agent is in one of the following modes: explore, improve localization, and improve map modes. Examples of the attractor placement in these modes are shown in Figure 25 (source: [18], p. 5029). a) Explore b) Improve Localization c) Improve Map Figure 25 Three modes of the exploration technique 59

60 Exploration points that are uniformly distributed in the environment are used to represent unexplored area. They are shown as blue dots on images in the figure above. As an agent covers an exploration point with its sensors, it is removed from the list of exploration points. Exploration is completed when there are no more exploration points. In the explore mode, the closest to the agent exploration point is set as the attractor. As a result, x and y coordinates of the chosen exploration point with associated covariance matrix are added temporally to the map. This artificial landmark has low pose confidence and, as a result, agent moves in its direction to better localize it. If the closest exploration point lies outside of the local area analyzed by the MPC planning strategy, the attractor is placed within the local area on the shortest path to the selected target point. The situation when the selected exploration point lies outside of the N-step distance is shown on the left image given in Figure 25 (source: [18], p. 5029). As it can be seen, the attractor has a big position uncertainty and it is located within the local area on the way to the selected exploration point. As soon as the agent s pose uncertainty overcomes the predefined threshold, an agent switches to the improve localization mode. In this mode, a landmark with good pose estimation is set as the attractor. Again, if this feature is outside of the local area, the attractor is placed within the range, analyzed by the MPC planning mechanism, on the shortest path to the selected feature. In the central image of Figure 25 (source: [18], p. 5029), the moment when the agent is in the improve localization mode is shown. Similarly to the previously discussed mode, the target point is outside of the local area. Therefore, the attractor is placed within the N-step distance to the agent, but in this case, the attractor s pose confidence is high to attract the agent. If the current goal is to improve the map, an agent is in the improve map mode. In this mode, the feature with bad pose estimation is chosen and it is set as an attractor. As in the other modes, if this feature lies farther than N-step distance, an attractor is placed within the local area on the shortest path to the selected feature. This mode is shown in the right image of Figure 25 (source: [18], p. 5029). MARKER-BASED EXPLORATION The technique proposed in [19] makes use of markers that are left on the ground to facilitate the localization problem. Each marker is equipped with a compass, a processor, and a small radio that allow to perform simple computations and to communicate some information. The purpose of a marker is to suggest to the agent that is within its communication range the direction to navigate. Each marker has the information of whether one of four cells that lies in north, south, east or west directions within its sensing range is explored and when was the last time it was visited. First, a marker recommends 60

61 the cell that is not explored. As soon as the area around the marker is entirely explored, it suggests the cell that was least recently visited. As an agent reaches the recommended point, it sends a signal to the marker that this cell is explored. An agent moves in the direction suggested by the marker, if there are no more markers within its communication range, it leaves a new marker in the environment. The process of markers deployments is shown in Figure 26 (source: [19], p. 2719). Markers are represented as black dots, the agent is represented a black square. GLOBAL PLANNING Figure 26 Deployment of markers The global SLAM exploration technique is discussed in [20]. Typically, one of parameters used to select the target point among present frontier cells is the information gain obtained upon reaching the frontier cell under study. The information gain is measured by the size of unexplored area that an agent covers with its sensors. Authors of the technique proposed in [20] suggest computing a sequence of steps that leads to the maximum information gain. At each step, the whole trajectory is recomputed again and a new target point is assigned to an agent. Authors state that the global planning strategy results in more accurate maps. However, the main disadvantage of this technique is its high computational cost that makes the already computationally heavy SLAM algorithm even more costly. Therefore, local greedy techniques that keep the computational real time are more popular. GREEDY TECHNIQUE The technique proposed in [21] is a greedy technique. A direction for the further exploration is defined by the cost function that consists of two main components the occupancy grid utility and the SLAM utility. The occupancy grid utility evaluates the expected information gain that is obtained upon reaching the target cell. The SLAM utility evaluates the expected change of the agent s pose uncertainty. If the pose uncertainty is too big, an agent is attracted to the previously seen landmark with high pose confidence. If an agent has a low pose uncertainty, it is attracted towards less explored area. This approach is stated to be an effective and computationally not expensive exploration technique. Due 61

62 to its advantageous and simple implementation, it was selected as a base for a single-agent SEIF SLAM exploration. This technique is described in details in the following section Single-agent Active SLAM algorithm The technique for the single-agent active SLAM exploration proposed in [21] was adapted for the SEIF SLAM algorithm described in the Chapter 1 of this thesis. In this section, the detailed description of the adapted technique is presented together with some preliminaries on the entropy and the information gain. PRELIMINARIES The information gain is one of the main components that constitutes the utility of the evaluated frontier cells. This measure is closely tied with another concept called entropy. Both concepts have their roots in the probability theory. The entropy characterizes the unpredictability in a variable. The formula used to compute the entropy for discrete systems is given below: Where a discrete distribution, an event space of a variable. ( ) From the formula, it can be noticed that the value of the entropy is always positive. Let us take a simple example assuming that there is only one cell and that it can be either occupied of free. In this case, the event space consists of two components. It is possible to prove that the maximum of the entropy is when the probability is uniformly distributed, in this case when the probability of the cell being occupied is 0.5. The corresponding max is: Consequently, the higher the degree of unpredictability, the higher the value of the entropy. The entropy is used to characterize the compactness of a given Gaussian distribution. The bigger the uncertainty, the higher the value of the entropy. An example of the value of the entropy depending on the covariance matrix is given in Table

63 Table 11 Entropy Covariance matrix Plot of the Gaussian distribution Entropy The information gain is given by the following formula: ( ) As it can be seen from the expression above, the information gain measures the difference in the entropy before and after the action took place. In our case, it measures the difference in the entropy before and after the control vector was applied. If before moving to a frontier cell an agent did not have any prior knowledge about the state of the area around the selected frontier cell (occupied/free), than the resultant information gain has a high value. Otherwise, if such a cell was already explored, an agent does not gain so much information and, as a result, the obtained information gain is small. ADAPTED SLAM EXPLORATION TECHNIQUE As it was briefly mentioned in the previous sub-section, the selected single-agent SLAM exploration technique is a greedy technique that evaluates only frontier cells that are a one-step distance from the current agent s position. 63

64 The choice of the target point depends on two components of the utility function: the SLAM utility and the occupancy grid utility. The latter one is defined by the estimated information gain. The SLAM utility, in turn, evaluates the change of the total certainty of the map that is defined by the sum of determinants of sub-matrices that are on the main diagonal of the covariance matrix. As we can notice, the SLAM utility forces an agent to revisit previously seen landmarks to relocalize itself and, in this way, decrease the global uncertainty of the map, while the other one pushes an agent towards the unexplored area. The importance of each of these utilities depends on the coefficient ( ). If this coefficient is set to 0.5, the accuracy parameter and the time parameter have equal priority. In the exploration technique hybrid map representation is used that means that two types of map representation are exploited: the occupancy grid map and the feature-based map. The first one facilitates the computation of the occupancy grid utility. The featurebased map is used to compute the SLAM utility by evaluating the change in the global map uncertainty after the action took place. The algorithm used for the implementation is given Figure 27. The algorithm takes as inputs the following variables: a probability matrix, a mean vector, an information vector, and an information matrix. Each component of a probability matrix corresponds to a particular cell of a map and is assigned a value between 0 and 1 that represents an occupancy probability of this cell. Initially, all the cells are given a uniform probability equal to 0.5 that can be interpreted as a state of global uncertainty. The function of the control generator block is to generate a set of control vectors that guide an agent in different points that lie on the circle with a radius equal to a length of the agent s one-step motion. One after another, all control vectors are applied to the system and the utility value associated with each control vector is computed. Clearly, the more control vectors are analyzed, the higher the probability that an optimal target point is selected but, at the same time, the higher is the computational cost. Therefore, a number of generated control vectors has to be chosen as a tradeoff between the optimality and the computational complexity. 64

65 THE SEIF SLAM PREDICTION THE CONTROL GENERATOR THE OG UTILITY THE SLAM UTILITY THE COMPOSITE UTILITY SELECTION THE SEIF SLAM UPDATE THE OG UPDATE Figure 27 Algorithm of the adapted single-agent SLAM exploration technique 65

66 As a control input is applied, the SEIF SLAM prediction block performs two-step computation. The first step is the SEIF SLAM motion update step that computes a mean vector, an information vector, and an information matrix. In the next step, the SEIF measurement update step is performed based on the expected measurement. In this case, since true measurements are not available, the difference between the true measurements and estimated ones is set to zero. Based on predicted values, a SLAM utility is calculated. A SLAM utility depends on the predicted change in the global map uncertainty. The global map uncertainty consists of a confidence in the agent s pose and in all detected landmark s positions. Two aspects influence a predicted change of the map uncertainty. The first aspect is a the noise introduced by the motion, hence, a control input that results in a straight-line motion leads to smaller increase of a global uncertainty as the noise introduced by the rotation of a vehicle is not included. The second aspect is the presence of a landmark in the area that is covered by the agent s sensing range upon reaching a target point. If a landmark has a high position confidence, an agent improves its localization. If this landmark has a low position uncertainty, an agent decreases the landmark s uncertainty. In both cases, the global map uncertainty is decreased. As an agent moves, the global uncertainty of the map is monotonically decreasing. This is due to the fact that the agent only gains the new information and never loses it. Global uncertainty is defined by determinants of sub-matrices associated with each landmark and an agent that lie on the main diagonal of a covariance matrix. The lower bound on the global uncertainty is defined by the determinant that an agent has when it detects the first landmark. In order to demonstrate a gradual decrease of the global map uncertainty in terms of determinants, a simulation where an agent moves in a circle trajectory was performed. Decrease of uncertainties of 4 landmarks were observed and the obtained result is shown in Figure 28. As it can be seen, determinants of analyzed landmarks monotonically decrease. An agent closes the loop at each 59th step, at this moment uncertainties are decreased the most, as a result, stairs-like behavior is observed. 66

67 The value of the determinant The Landmark N 4 The Landmark N 3 The Landmark N 2 The Landmark N The number of steps Figure 28 Decrease of the global map uncertainty The formula used to compute the SLAM utility is a following: ( ) Where the agent-agent covariance sub-matrix, the covariance sub-matrix associated with the j-th landmark; the total number of detected landmarks. In the original exploration technique, it is proposed to compute the occupancy grid utility as the expected information gain. However, as it comes from the definition, the entropy (that is used to compute the expected information gain) itself is a measure of how unpredictable/uncertain the state of the map is and, therefore, can be used to evaluate the optimal frontier cell. It is a common practice to use the entropy instead of the expected information gain in order to decrease computational cost. Therefore, it is suggested to compute the occupancy grid utility as a sum of the entropy of each cell that the agent covers with its sensors when in a vicinity of the target cell: 67

68 ( ) Where the entropy of the j-th cell, the area covered by the sensing range. In the next block shown in Figure 27, the composite utility is computed using the following formula: Where ( ), weights that normalize the composite utility and assign the priorities to the time and the accuracy parameters of the exploration. Formulas used to compute these weights are given bellow: ( ) ( ) The coefficient has a constant value between 0 and 1 and defines the priority that the time and the accuracy parameters have. If is set to 0.5, both parameters of the exploration technique have equal priority. If is greater than 0.5, the accuracy of the resultant map has higher priority. Clearly, If is smaller than 0.5, the speed of the exploration has higher priority. Maximal values of the SLAM utility and the occupancy grid (OG) utility are used to normalize the composed utility. The OG utility has a constant value and, therefore, it is computed only once at the beginning of the algorithm. Its value depends on the maximum number of cells that are covered by the agent s sensors. To compute the maximum OG utility, all these cells are set to 0.5 that correspond to the global uncertainty and, as a result, to the biggest OG utility. The value of the maximum SLAM utility varies until the moment all the landmarks are detected. As it was discussed, the lower bound of the map uncertainty is defined by the confidence of the first detected landmark. Following the same logic, the upper bound of each landmark is defined by its determinant at the moment it was observed for the first time. Therefore, each time a new landmark is detected, the maximum SLAM utility is recomputed. 68

69 The purpose of the selection box shown in Figure 27 is to compare all the computed composed utilities and to choose the one that has the highest value and the optimal control vector associated with it. At this point different control inputs were evaluated and now the selected optimal control vector is applied to the system. Once the measurements are taken the measurement update step, the sparsification step, and the mean recovery step are performed. The occupancy grid (OG) update block updates the occupancy probabilities of all the cells within agent sensor range based on the Bayes rule: ( ) Where the previous occupancy probability of j-th cell within the area covered by the sensing range, a probability of taking the measurements, given the current state of the j-th cell is occupied. In order to compute this probability, the following formula is used: ( ) Where components of the measurement covariance matrix, taken measurements; measurements that would be taken assuming that the cell j is occupied. This formula represents a multivariate Gaussian with diagonal covariance matrix: this means that the two variables and are assumed to be statistically independent. 69

70 Simulation results This subsection provides the results of the simulation of the exploration technique adapted to SEIF SLAM. The simulated environment is shown in the upper-left corner of Table 12. As it can be seen, the environment is discretized into cells. The color of each cell represents its occupancy probability: white free cell, black occupied cell, grey unexplored cell. Initially, each cell has a probability of being occupied 0.5. The agent starts exploration in the position (0,0) and with the orientation 0 [rad], its initial position is shown as a black cell. The value of is set to 0.5 that corresponds to the exploration with the equal priority given to the time and the accuracy parameters. Table 12 Stages of the single-agent exploration Initial state of the map Number of performed steps = 25 Number of performed steps = 50 Number of performed steps =

71 As the agent moves, the area that is covered by its sensing range is modified: free cells are shown with a white color, while dark colors are used to show occupied cells. That is the reason why, the area around the landmark consists of dark-colored cells. As it can be noticed, an agent was navigating towards unexplored area as it was expected. However, at the step 135, the agent got stuck and started to oscillate between two positions. This is due to the following reason: as soon as the area around the agent is explored, the OG utility that was driving it to the closest unexplored area at that moment had the same small values for all analyzed frontier cells. As a result, only the SLAM utility affects the choice of the optimal control law. Clearly, it tries to keep an agent close to the landmark to prevent an increase in its pose uncertainty. The reason for this problem is a greedy strategy that analyzes only frontier points within one-step distance from the agent. In the following sub-section, the way to overcome this problem is proposed, together with additional improvements that are not taken into account in the original exploration technique Improvements of the single-agent Active SLAM algorithm DEALING WITH THE LOCAL TRAP The problem of the local trap that was revealed during the simulation is due to its greedy one-step planning strategy. One of the possible solutions to overcome this limitation could be the introduction of the global planning strategy that would analyze all the cells. However, as it was already mention, this policy makes the technique very costly. Therefore, the following idea is suggested: the technique remains unchanged until the moment an agent gets stuck; as soon as it happens, the policy is changed to the global planning one. First, cells that lie in the two-step motion distance are analyzed to allow an agent to go out of the trap. The distance is gradually increasing until the target point within unexplored area is selected. In order to prevent the agent from oscillating, it is necessary to add a suitable condition to detect the oscillation. In the original technique, at each time step only the current agent position is known. A vector that consists of the agent s last N positions is introduced. The following condition is exploited: if the optimal selected control input drives an agent close to one of its last memorized positions, the local one-step policy is switched to the global one. 71

72 OCCUPANCY MAP UPDATE STEP It is suggested to update the occupancy map with regard to agent s pose uncertainty that is not taken into account in the original algorithm. The suggested occupancy map update step is described below: 1) A certain number of samples is generated from a Gaussian distribution parameterized by the current estimated agent s mean position and the corresponding covariance matrix. The example of the histogram associated with selected samples is shown in Figure 29. In this example, the mean value is, the covariance matrix is, the number of samples is As it can be seen, the majority of samples are located close to the mean value. Figure 29 Histogram of the noise distribution In Figure 30, distributed samples for two different covariance matrices ( ) are shown. As it is expected, the covariance matrix significantly influences the choice of samples and there are more of them within the corresponding uncertainty ellipsoid Figure 30 Distributed samples

73 2) After a set of samples has been selected, for each sample the following procedure is performed: the number of times the cell i is covered ( ) by the agent s sensing range is computed. Initially, each cell has. A random sample is selected and the area that the agent covers assuming this sample corresponds to its true position is selected. is increased by 1 for each cell that is within this area. An identical procedure is performed for each sample. After these procedures have been done, the probability of being covered is calculated for each cell that has the associated number of being covered different from zero. This probability is equal to the number of times it was covered over the total number of samples: ( ) In order to demonstrate the effect of the agent s position uncertainty on the occupancy map update, some simulations were performed. The state of the updated occupancy map is analyzed depending on the covariance matrix of the agent s position. Initially, the map is unknown and each cell has a uniform probability of being occupied: 0.5. The simulation results are demonstrated in Table 13. In the left column, the used covariance matrix is given. In the right column, the updated occupancy map is shown where a red circle represents the sensing range that wass set to 4. The number of used samples was 100. As it can be seen from the results, as the position confidence decreases with the growth of the covariance matrix, the occupancy map update is performed with smaller confidence. Table 13 State of the map depending on the position confidence Covariance Matrix Updated Map 73

74 The influence of different numbers of samples on the occupancy map is given in Table 14. The sensing range was set to 4, the covariance matrix was the following:. Table 14 State of the map depending on the number of samples N 1 Selected samples Updated Map 10 74

75 From the given plots, it can be noticed that 10 samples lead to very unsmooth updated occupancy map. The map with 50 used samples looks much smother. Both 100 and 1000 samples lead to a very similar occupancy map. Therefore, for this case, 100 samples are enough. The choice of the appropriate number of samples depends on the confidence of the agent s position: the bigger the uncertainty, the more samples are required to obtain smooth updated occupancy map. Simulation results with the covariance matrix that is bigger than in the previous simulation are shown in Table

76 Table 15 State of the map depending on the number of samples N 2 Selected samples Updated Map

77 1000 As it can be seen, around 1000 samples are required to obtain a smooth updated map, while in the previous case 100 samples are enough. THRESHOLD According to the original technique, the occupancy map is updated at each step. As an agent moves, its pose confidence decreases due to the motion uncertainty. Clearly, when an agent s pose is very uncertain, the obtained information about the state of the environment is not reliable and, therefore, it leads to significant inconsistence between the updated occupancy map and the true map of the environment. This important aspect is not taken into account in the original technique. In order to manage this situation, it is proposed to introduce a threshold on the agent s pose uncertainty. As soon as an agent s pose uncertainty overcomes the predefined threshold, the strategy is switched from the local to the global planning one. In this case, an agent navigates to the closest landmark with a high position confidence in order to reduce the uncertainty in its pose. While the agent moves and its uncertainty grows, the map is updated with less confidence. At the same time, the feature-based map update is not affected. The effect of the threshold introduction is shown in Figure

78 The agent s pose uncertainty has reached the predifined threshold The agent revisits a feature with a good position confidence to improve its localization Figure 31 Effect of the threshold introduction As it can be seen, the described behavior is achieved: as the agent s pose uncertainty grows, the occupancy grid map is updated with less confidence. In the moment shown on the left map, the agent s pose uncertainty reached the level of the predefined threshold. Consequently, the planning strategy is switched to the multi-step mode in order to find the closest feature with a high position confidence. The agent navigates to the selected feature (yellow arrow in the right plot of Figure 31). The area around the agent s estimated pose is zoomed in to show the result of the pose uncertainty decrease. The predicted uncertainty is shown as the blue ellipsoid. After the selected landmark has been detected, the pose uncertainty is decreased (the violet ellipsoid). As soon as the agent improves its localization, the planning strategy is switched back to the one-step strategy Simulation results This sub-section presents the simulation results of the improved single-agent SLAM exploration technique. The environment that was used for the simulation is shown in Figure 32. The map consists of 10 landmarks and is divided into 0.5x0.5 cells. The number of active landmarks was set to 3. The sensing range was 3. The following covariance of the measurement uncertainty was used:. 78

79 Figure 32 Environment used for the simulation COMPARISON WITH OTHER EXPLORATION TECHNIQUES Results of running many times the improved single-agent exploration technique have shown that the problem with local traps was overcome. The effectiveness of the exploration technique, in terms of the time and the accuracy of the resultant map, was compared to the random walk and the snake-like exploration techniques. The random walk technique does not require any planning strategy as, at each time step, a set of control inputs is generated and one of them is randomly selected. Different stages of the snake-like technique are given in Figure 33. The agent starts moving straightforward from the lower-right corner. It follows the linear trajectory until it detects a wall, then it rotates on 180 degree and moves straightforward again. step N 25 step N 44 step N 138 Figure 33 Stages of the snake-like exploration technique 79

80 The comparison of the average time required to complete the exploration mission is given in Table 16. As it was expected, the snake-like technique has the fastest speed of the exploration. The worst result was obtained by the random walk exploration technique; this is due to its tendency to a redundant coverage. In the simulated case, the number of steps to complete the exploration varied from 986 to 1857 steps. In the improved exploration technique, the agent controls the level of its pose uncertainty and as soon as the uncertainty overcomes a predefined threshold, it revisits previously detected landmarks to improve its localization. Due to the additional time spent to travel back to the landmark with a good position confidence, the improved exploration technique is less effective, in terms of time, than the snake-like technique. Table 16 Comparison of the time parameter Exploration technique Average number of steps Snake-like 181 Improved exploration 263 Random walk from 986 to 1857 However, revisiting previously seen landmarks leads to the higher map accuracy. The results of the snake-like and the improved exploration techniques comparison, in terms of the resultant average error of the map is given in Table 17. The resultant average error is computed based on ( ). Both techniques were tested with different values of the covariance matrix of the motion model. Where total number of detected landmarks, ( ),,, estimated and true x,y-coordinates of the i-th landmark respectively. N 1 Covariance matrix of the motion uncertainty Table 17 Comparison of the accuracy parameter Exploration Average feature-based technique map error Improved exploration 0.38 Snake-like

81 2 3 4 Improved exploration 1.41 Snake-like 3.92 Improved exploration 3.87 Snake-like 8.10 Improved exploration 6.10 Snake-like As it can be noticed, as the uncertainty of the motion model grows, the resultant average map error increases. These results demonstrate that the improved exploration technique has performed better in all tests. The uncertainties of the resultant maps that correspond to cases N 2 and N 3 are shown in Table 18. Table 18 Resultant map uncertainty N Snake-like Improved exploration

82 OCCUPANCY MAP CONFIDENCE As it is described in Section 2.2.3, the update of the occupancy map takes into account the uncertainty of the agent s pose. The influence of the agent s pose uncertainty on the confidence of the resultant occupancy map is analyzed. Resultant occupancy maps obtained by the improved exploration and the snake-like techniques are compared. Simulation results are given in Table 19. Map confidence computation is based on the following formula: Where occupancy probability of the cell i, - total number of cells. ( ) Table 19 Occupancy map confidence Technique Improved technique Snake-like technique Covariance Time Occupancy map confidence 99.8%, 74.6% Resultant Map Covariance Time Occupancy map confidence 99.5%, 33.8% 82

83 Resultant Map Covariance Time Occupancy Map confidence 99.8% 16 % Resultant Map The obtained data shows that, in case of the snake-like technique, the continuous increase in the pose uncertainty results in poorly explored area with low occupancy map confidence. In case of the improved exploration technique, the confidence of the occupancy map remains independent from the covariance of the motion uncertainty. This happens due to the introduced threshold that defines the level of the allowed agent s pose uncertainty. As soon as the pose uncertainty overcomes the predefined threshold, the agent revisits previously seen landmarks and in this way improves its localization. As a result, the occupancy map is updated with limited lower bound of confidence. It can be also noticed that the confidence of the resultant feature-based map is much higher than the one obtained by the snake-like exploration technique. This is due to the loop-closure that is performed in the improved exploration technique: the agent improves its localization by revisiting landmarks with good position confidence and in this way reduces the global feature-based map uncertainty. 83

84 Number of steps THRESHOLD The influence of the threshold on the time (expressed in number of steps) required to complete the exploration is shown in Figure Threshold Figure 34 Exploration time When the value of the threshold is small, the agent has to revisit previously seen landmarks often, which leads to longer coverage. As the threshold increases, the time decreases. However, when the threshold is too big, even though the agent does not have to spend time to improve its localization by revisiting landmarks, the total time of complete coverage grows. This happens for the following reason: if the agent does not improve its localization, its continuously growing pose uncertainty becomes very large and, as a result, the occupancy map is updated with small confidence, consequently, the agent has to cover again the poorly explored area that leads to the increase of the total exploration time. SPEED The number of steps required to fully explore the environment depending on different values of other parameters was analyzed. In the first simulation, the effect of the sensing range on the speed of the exploration was evaluated. The results are provided in Table

85 Table 20 Total number of steps (sensing range) Sensing range Total number of steps The obtained data demonstrates that with bigger range the total number of steps decreases as it was expected. In the following simulation, the time parameter was evaluated depending on the motion model uncertainty. The threshold of the agent s pose uncertainty was equal in all these simulations. The results given in Table 21 show that as the uncertainty of the motion model grows, the total number of steps increases. This is due to the fact that a bigger motion uncertainty leads to more frequent travel towards the landmark with a high position confidence in order to improve the agent s localization. Table 21 Total number of steps (motion uncertainty) Covariance matrix of the motion model Total number of steps In the third simulation, the influence of the threshold on the efficiency of the exploration is examined. The efficiency was evaluated in terms of the time required to complete the exploration mission. The data given in Table 22 shows that the lower the threshold, the more frequently the agent has to revisit previously observed features with good position confidence, which leads to a longer exploration. 85

86 The average error of the map Table 22 Effect of the threshold Threshold Average time of the exploration ACCURACY It was also observed that the more steps are performed the lower the value of the average error (22.4.1) due to more frequent loop-closure and, therefore, the higher the accuracy of the feature-based map. As it was discussed, the global uncertainty of the map monotonically decreases: the more steps are performed, the more information about the environment is obtained. The plot given in Figure 35 graphically shows this observation The number of steps Figure 35 Average map accuracy error 86

87 2.3. Active SLAM for a multi-agent case The analysis of the state of the art of Active SLAM for a multi-agent case has shown that this area is not very well explored. The majority of papers that address multi-agent systems are focused on the problem of map merging, in case of unknown data association, and they consider systems where the agents start the exploration from distant locations that lie outside of the communication range and, therefore, each agent has its own coordinate frame. In these cases, before a map merging strategy can be applied, the transformation of one coordinate frame into the other has to be performed and this, admittedly, is not an easy task with the presence of pose and measurement uncertainties. In the considered in this thesis case, we assume that agents start exploration close to each other and, as a result, they share the common coordinate frame, which significantly simplifies the task. There are several papers [22],[22], [23] that address the problem of the multi-agent Passive SLAM where agents are passive and there exists a central block that assigns a task to each of them. The only paper (to our best knowledge) that addresses the active multiagent exploration SLAM problem is [24]. The authors consider the case of the EKF SLAM exploration that as we discussed previously is a very costly SLAM algorithm and cannot be applied for large environments. When agents are within the communication range, they propose to use an assignment matrix that is used to assign a particular target cell to each agent based on the cost function that consist of the SLAM utility, OG utility, and cost of travelling. Therefore, agents are constantly tied with other teammates and the process of selecting target points is centralized and leads to increase of the computational cost as the number of agents grows. Therefore, it was decided to take as a base the improved single-agent exploration SLAM technique suggested in the previous sub-section and extend it to the multi-agent case. In this section, two multi-agent SLAM exploration strategies are described. The first strategy is under the assumption of unlimited communication. The second is a more advanced one and it takes into consideration the communication bounds. The case of the two-agent system is addressed in this thesis. However, both techniques can be without troubles extended to a bigger number of agents. 87

88 Multi-agent SLAM algorithm with unlimited communication range In this part, a strategy for a multi-agent exploration under the assumption of the unlimited communication range is discussed. This assumption allows us to straightforwardly extend the single-agent exploration technique described in Sections and to a multi-agent case. As two agents are always within the communication range, the information obtained by agents can be exchanged at any moment. However, the constant exchange of information is unnecessary and, obviously, leads to bigger energy consumption and longer exploration due to the time spent for the communication. Therefore, it is suggested to use a threshold that represents the minimum number of steps each agent has to perform before exchanging the map. Each time the agents meet, they perform a map integration, which results into a merged map with lower uncertainty. The technique of maps collapse described in Chapter 1 is exploited Simulation results In this part, the results of simulations of the suggested techniques are given and discussed. The environment used to simulate the first multi-agent SLAM exploration strategy is shown in Figure 36. It is divided into 0.5-by-0.5 cells and consists of 26 landmarks the true locations of which are shown as red stars. The initial positions of agents are shown as black cells. The following parameters were used for the simulation: the number of active landmarks was set to 3, the sensing range is equal to 2, and the measurement uncertainty had the following values:. 88

89 Figure 36 Simulation environment for a multi-agent case Several simulations were performed to verify the correctness of the implemented code; the observed behavior appeared to be as expected. Several tests were performed to analyze the effectiveness of the multi-agent system in terms of time and accuracy compared to the one of the single-agent exploration. Both systems were run with equal initial parameters. In the first test, the time required to complete the exploration mission with different values of the threshold on the agent s pose uncertainty was compared for both systems. The results of this test are given in Table 23. Table 23 Single-agent vs. Multi-agent exploration (threshold) System Threshold Time Single-agent system /400 Multi-agent system 99 Single-agent system /800 Multi-agent system 108 Single-agent system / Multi-agent system 157 Single-agent system / Multi-agent system 237 From the obtained data, it can be seen that as the value of the threshold is decreased the number of steps required to complete the exploration increases simultaneously. This 89

90 happens due to the need to re-visit more often landmarks with good position confidence to improve the agent s localization. The speed of the exploration of the multi-agent system was always higher. Similar results were obtained in the second test, where the sensing range was varied (Table 24). From the obtained data, it is clearly seen that the multi-agent system is more efficient in terms of time. With the growth of the sensing range, the time for completing the exploration reduces. Table 24 Single-agent vs. Multi-agent exploration (range) System Sensing Range Time Single-agent system Multi-agent system 332 Single-agent system Multi-agent system 99 Single-agent system Multi-agent system 94 Single-agent system 86 5 Multi-agent system 53 In the last test, the accuracy of the resultant map was compared depending on the motion uncertainty. As the data given in Table 25 shows, smaller pose uncertainty results in better accuracy of the map. The time required to complete the exploration increases with when the uncertainty increases, because of more frequent re-visits of features with good position confidence. Table 25 Single-agent vs. Multi-agent exploration (uncertainty) System Motion uncertainty Time Accuracy Single-agent system Multi-agent system Single-agent system Multi-agent system Single-agent system Multi-agent system

91 Multi-agent SLAM algorithm with limited communication range Now, the attention is shifted towards multi-agent systems with limited communication range. Several questions have to be addressed in order to select an appropriate tactic. The most important one is how to deal with the communication bound. In order to answer this question, the possible options as well as a brief discussion of their weak and strong sides are given. Currently, there exist two ways to handle the communication limitation: - keep agents always within the communication range, - allow agents to go beyond the communication range; In the first case, agents always share the common map, which simplifies the map integration procedure. The other advantage of this approach is that the coordination of agents can be easily implemented and the problem of the redundant coverage is easily overcome. However, this approach generally leads to a longer exploration, because since agents have to keep the communication, this may force some of them to visit areas that are already explored. In the second case, agents can go out of the communication range and explore the environment independently. Nevertheless, the degree of freedom that the agents possess has to be defined. The approach where agents are completely independent from each other and can freely navigate in the environment inevitably leads to an overlapping coverage and, as a result, a longer exploration. The rendezvous technique overcomes this problem. It is based on the following idea: agents are allowed temporary to go out of the communication range and after specific time they meet in a previously agreed rendezvous point. When agents meet, the map integration is performed, which results in an increase of the global confidence. As this technique combines the advantages of both described approaches, the rendezvous strategy is selected for the multi-agent SLAM exploration. The second question to address is how to select the rendezvous point. Initially, since the area is fully unexplored the meeting point is set in the area where agents start the exploration mission. Before they switch to the independent exploration phase, besides selecting the meeting point, they need to agree on how much time they will proceed independently before going to the rendezvous point. In our case, the time is simply translated in the number of discretized steps. As they go back to the independent exploration, each of them counts the number of steps they have done and computes how 91

92 many steps it needs to reach the meeting point. As soon as the time runs out, each agent memorizes its current position and navigates towards the meeting point. When agents meet, they exchange the information that was gained, perform map integration, and decide upon the next meeting point and time. This time, the rendezvous point is set in the explored area that lies preferably in the middle between the agents last positions before they directed towards the meeting place. The number of steps is also changed; it depends on the maximum number of steps the agents need to go back to their last positions plus the predefined number of steps that is set in the beginning of the algorithm. If agents meet accidentally, the new meeting point is set between their current positions and the counter is reset. Furthermore, a distance utility function that serves to navigate agents in different directions was introduced. It is defined as a function of the distance between agents: the bigger the distance the higher the value of the utility function. The maximum value of the distance utility function is equal to the communication range. In the independent exploration mode, each agent is navigating in the environment using the improved single-agent exploration technique described previously Simulation results The environment that is used for the simulation is shown in Figure 37. The big map represents a common map, while the two smaller ones belong to each agent. The environment is divided into 0.5-by-0.5 cells and consists of 26 landmarks the true locations of which are shown as red stars. The initial positions of agents are shown as blue circles. The yellow diamond represents a rendezvous point. The map is shown in brown color when agents are within the communication range, while when agents are outside of the communication range, it is shown as a gray scale map. The following parameters were used for the simulation: the number of active landmarks was set to 3, the sensing range equals 2, the number of steps before the next meeting was set to 10, and the communication range was set to 6. 92

93 Figure 37 Simulated environment for a multi-agent case Each agent starts exploration independently and moves towards the unexplored area. In Figure 38, the moment before agents switched to the rendezvous mode is shown. As it can be seen, each agent holds its own occupancy grid map. 93

94 Figure 38 Independent exploration mode In the rendezvous mode agents navigate towards the a-priori set meeting point. As soon as they meet, they exchange their occupancy maps to create a common one and perform maps collapse. The moment when two agents met is shown in Figure 39. As it can be noticed, all three occupancy grid maps are equal, as a result of the information exchange. After agents decide upon where and when to meet next time, they switch back to the independent exploration mode, and move towards the positions they have occupied before the rendezvous mode. 94

95 Figure 39 Rendezvous mode Performance of the two strategies was compared based on the time required to complete the exploration. The results are provided in Table 26. The results demonstrate the expected behavior: the time required to finish the exploration is greater in the case of the strategy that deals with limited communication range due to its additional time spent to meet with another colleague. Table 26 Performance comparison (time) Strategy Time to complete the exploration Unlimited communication range 362 Limited communication range