Thesis Outline

Chapter 2 illustrates the coordinate transformations between different frames; the SLAM system description is also presented. Standard EKF and UKF technique is described in Chapter 3, and its associated equations are listed. In Chapter 4, the map building based on line segment is described. Chapter 5 simulates the SLAM scenario.

Chapter 6 consists of summary and conclusion.

CHAPTER 2

FUNDAMENTAL CONCEPTS

2.1 Simultaneous Localization and Map Building (SLAM)

The problem of localization given a map of the environment or estimating the map of knowing the mobile robot position is known to be a solved problem, and in fact applied in many research and industrial applications [Durrant-Whyte , 1996]. A much more fundamental problem is when both the map and the mobile robot position are not known. This problem is usually referred as Simultaneous Localization and Map Building (SLAM) [Guivant et al., 2000]. When absolute position information is not available, it is still possible to navigate with small errors for long periods of time. The SLAM algorithm addresses the problem of a mobile robot with known kinematics, starting at an unknown position and moving through an unknown environment populated with some type of features. The algorithm uses dead reckoning and observation to estimate the position of the vehicle and to build and maintain a navigation map. With appropriate planning, the mobile robot will be able to simultaneously navigate and build a relative map of the environment. If the initial position is known with respect to a global reference frame or if absolute position information is obtained during the navigation task, then the map can be registered to the global frame. If not, the mobile robot can still navigate while exploring and incorporating new areas to the map.

2.1.1 The Dynamic Localization Problem

Robot localization has been widely acknowledged as one of the most fundamental problems in mobile robotics. The aim of localization is to estimate the position and orientation (briefly, pose) of a robot in the environment, given a map of the surrounding space and sensor data. Most successful mobile robot systems to date utilize localization techniques, since knowledge of the robot pose is essential for a broad of robotic tasks.

Although investigated extensively during the last three decades, the localization problem is still an active field of research because of the lack of a single, generally appropriate method. As it comes out from surveying the vast body of literature on mobile robot positioning, to date three is no universally valid solution. The reader interested in the most common positioning systems is referred to [Borenstein et al., 1996] and reference therein, where commercially available sensors and integrated systems for mobile robot localization are thoroughly reviewed.

Despite the heterogeneity of the solutions found in literature, the localization methods proposed so far can roughly be categorized into local and global techniques [Fox et al., 1999]. Tracking, or local, techniques incrementally update the robot position.

They require the knowledge (at least approximate) of the initial robot pose of the autonomous vehicle and they cannot typically recover from a possible failure of the robot position tracking. On the contrary, global techniques are designed to estimate the robot pose even under global uncertainty, i.e. when the initial robot configuration is completely unknown. Clearly, techniques falling in the latter category are more powerful than the former, paying the price of higher computational and storage requirements. For instance, they are able to face the so-called “wake-up robot problem”,

associated with finding an estimate of the pose from scratch, as well as the “kidnapped robot problem”, in which a robot is carried to an arbitrary location during its operation.

Global localization methods do not need any kind of initialization of the estimate and can cope with circumstances in which the robot is likely to experience serious position errors.

2.1.1a Local Techniques

The simplest method for tracking the position of an autonomous vehicle is dead reckoning. Derived from “deduced reckoning” of sailing days, it denotes a mathematical procedure for determining the current position of a robot by some previous position through known course and velocity information, over a given length of time. In its easiest implementation, such a technique is usually referred to as odometry, since the position of the vehicle along its path is directly provided by some on-board “odometer.”

Optical encoders, directly coupled to the wheel axes, are so far the most common sensors suited to this task. Because of the easiness of the computation involved and the inexpensiveness of the sensory system required, practically all mobile platforms are supplied with odometric modules. A more elaborate dead reckoning implementation is given by the Inertial Navigation Systems (INS) [Leonard and Durrant-Whyte, 1992].

Position estimate is computed by integrating measurements of rate of rotation and linear acceleration, provided by gyroscopes and accelerometers. Both odometry and INS are prone to systematic and stochastic errors. The former are mainly due to uncertainty in the kinematic model of the vehicle or inaccuracy in the sensor model. For example, the distance between the wheel axes, as well as the wheel radii, are known only up to a

certain degree of accuracy. The latter are the resulting effect of the noise corrupting the measurements. Unfortunately, the integration process, underlying the principle of dead reckoning, leads to error accumulation. Although off-lone calibration procedures, aiming at minimizing the effect of systematic errors on odometric systems, have been proposed [Borenstein and Feng, 1996a], unless additional information from some absolute position sensing mechanism is used, the position error grows without bound, as the robot moves on. Nevertheless, dead reckoning still represents an effective solution for tracking a mobile robot, as far as short-rang navigation is concerned. Since stochastic errors cannot be compensated off-line, filtering is usually introduced to estimate the kinetic quantity to be integrated. For example, a remarkable improvement in the performance of a low cost INS can be achieved if error models of the inertial sensors are generated and included in an Extended Kalman Filter (EKF) [Barshan and Durrant-Whyte, 1995]. Odometric techniques and INS can also be exploited together. In [Borenstein and Feng, 1996b] a method using encoders and gyroscope during different time intervals, is proposed. The aim is to compensate for non systematic odometry errors and, at the same time, to keep the tracking free from the drift associated with the gyroscope. A complementary Kalman filter can be effective to estimate the vehicle’s attitude from the accelerometer signal in low frequency motion and from the gyro signal in high frequency motion.

To overcome the long-term drift associated with dead reckoning problem, the most common approach consists of periodically reset ting the estimation error, on the basis of some kind of absolute measurements, i.e. taking with respect to an external reference frame. Mobile robots are usually equipped with several sensors, to avoid the limitation

of a single measurement system. They are generally distinguished by proprioceptive and exteroceptive sensors. For those sensors whose measuring quantities related to the robot itself (encoders, gyroscopes, accelerometers, compasses), belong to the former while proximity sensors (infrared, sonars, laser range finders, millimeter wave radars), CCD camera, stereocams, and all other sensors performing measurements on the external environment fall into the latter. Fusion of the two kinds of information is usually obtained via some filtering technique, the EKF being the most used [Bar-Shalom and Fortmann, 1988]. In this framework, the uncertainty on robot pose is modelled as an unimodal Gaussian distribution, whose mode represents the pose estimate and whose covariance matrix gives a measure of the estimation uncertainty. [Leonard and Durrant-Whyte 1991] developed a localization algorithm employing EKF to match beacon observations (extracted from sonar scans of the surroundings) to a priori map, given in terms of cylinders, corners, and planes. EKF is used to fuse angular measurements related to light sources, detected using a CCD camera, with the information coming from the odometry. [Cox, 1991] matches distance taken by infrared sensors to a segment description of the robot’s environment, while [Crowley, 1989]

explicitly represents the uncertainty on the vehicle position as well as the uncertainty inherent in the sensing process. Line segments extracted by a belt of ultrasonic range devices are used to correct the robot pose estimates by matching, via EKF, the extracted feature to a model of the environment. In [Neira et al., 1999], a specialized version of the EKF, called SPfilter, is developed to estimate the location of a mobile robot from a set of partial and inprecise observations of features in the robot’s environment.

All the methods based on the EKF generally models the uncertainty affecting the

measurement process and the vehicle dynamics as zero-mean, white Gaussian noise.

Real-world uncertainties seldom satisfy these hypotheses. Moreover, even when these assumptions are fulfilled, the kalman filter is guaranteed to converge, but the EKF is not.

Special caution is considered when neglecting strongly correlated noise or systematic errors. In those cases, the Kalman filter estimates tend to be overoptimistic [Julier and Uhlmann, 1997]. These considerations motivated the adoption of alternative data fusion techniques. To cope with non-white, non-Gaussian noise, [Hanebeck and Schmidt, 1996]

propose a set-theoretic approach to the problem of tracking a mobile robot based on angular measurements. Under the hypothesis of bounded errors, information fusion is obtained via set intersections. This work has been later extended to a mixed stochastic/set-theoretic framework], in which the ellipsoidal approximations of the admissible robot poses are uncertain in a statistical sense. In [Jaulin et al., 2000] the problem of robot localization, under the hypothesis of bounded errors, is addressed via interval analysis, the optimal solution being computed via set inversion. The main drawback of this technique is the excessive computational burden, making the proposed algorithm unsuitable for real-time applications.

2.1.1b Global Techniques

Depending on the representation of the robot’s surroundings, several global localization methods have been developed. One of the most common descriptions of the robot’s environment consists in a map that are given in terms of landmarks. According to the definition of [Levitt and Lawton, 1990], a landmark is a distinctive visually event, defining a single direction and being visually reacquirable. More generally, a landmark

denotes a remarkable feature of the environment which, depending on the sensory system, the robot is to successfully recognize in successive time instants. They are usually distinguished in artificial and natural landmarks. Natural landmarks can be thought of as objects or features that are already present in the environment and have a function other than robot navigation. Artificial landmarks are specially designed objects or markers placed in the environment with the sole purpose of enabling robot navigation.

The latter kind of landmarks requires an engineering of the environment, while the former calls for efficient algorithms of feature extraction. Apart from the qualitative approach by [Levitt and Lawton, 1990], most techniques exploit angular and/or distance measurements with respect to landmarks having known position a prior, to compute an estimate of the current robot pose. [Sugihara, 1988] proposes a localization method based on the relative directions of natural landmarks (vertically oriented parts of fixed objects such as doors, desks and wall junctions) extracted from a single image, whose complexity is O( n³ log (n) ). Where n is the number of landmarks in the map. [Krotkov, 1989] extends the above technique by taking into account errors affecting angular measurements, performed via CCD camera. In both approaches, the algorithm is formulated as a search in the Interpretation Tree [Grimson and Lozano-Perez, 1987].

In order to reduce the computational burden of those methods, [Sutherland and Thompson, 1994] suggested a way of choosing the best triple of landmarks (supposed to be distinguishable), with respect to which localize the robot, exploiting relative angular measurements and implicitly assuming bounded noise. Representing the landmark coordinates as a complex number, [Betke and Gurvits, 1997] proposed a least-squares estimation technique, whose complexity is linear in the number of landmarks. Also in

this case, angular measurements with respect to distinguishable landmarks are processed.

More recently, inspired by previous results based on Interpretation Tree, [Mouaddib and Marhic, 2000] developed a localization methods based on goniometrical observations, taken by a panoramic camera, of indistinguishable landmark. The robot’s evolution field is iteratively subdivided into rectangles, as long as they contain at least one admissible landmark/measurement matching, until the desired estimation quality is achieved. To perform an efficient search in the Interpretation Tree and reduce the computational burden, the authors impose angular constraints. The same heuristics was already proposed in [Atiya and Hager, 1993], where a set-based algorithm, for solving the matching problem and computing the robot pose using a stereocam, is presented. A major characteristic of this technique is its ability to provide worst-case, distribution-free error analysis, since bounded measurement noises are assumed.

A different description of the environment is represented by the occupancy grids.

Developed by [Moravec and Elfes, 1985], they discretized the robot’s space into cells, marked by a score ranging from -1 to 1, representing the belief that the corresponding region is occupied (-1 denotes a certainly empty cell while 1 denotes a certainly occupied one). In this setting, the robot pose can be estimated by matching a local map, built from the current sensor readings, against a prior known map. A measure of the goodness of the match between two maps, and hence a trial displacement and rotation, is found by computing the sum of products of the corresponding cells [Elfes, 1987].

Among other probabilistic methods, those proposed in [Thrun et al., 2000a] have gained remarkable attention. These techniques represent a first application of Particle Filters to mobile robotic problems [Thrun et al., 2002a]. The main idea is to

approximate the posterior of the robot pose by means of a fixed number of sample poses, or particles, generated according to probabilistic models of the robot dynamics and of the measurement process. Appealing properties of this approach are that it is able to deal with arbitrary distributions and the update of the estimates, given the observations can be carried out in constant time (it depends only on the cardinality of the particle set) and its accuracy increases with the availability of computational resources.

2.1.2 Map Models

Obviously, the SLAM techniques found in literature, strongly depends on the environment description adopted. Nonetheless, all the proposed SLAM solutions can be roughly categorized into three main approaches: grid-based, topological, and feature-based. The reader interested in the most common environment representations is referred to [Thrun et al., 2002b] for a comprehensive overview of the state of the art in robotic mapping.

2.1.2a Grid-Based Aapproach

The grid-based techniques represent the simplest approach to map building. They adopt probably the most intuitive environment representation, i.e. an occupancy grid.

Instead of using a set of lines or points to represent objects in the mapping environment.

The space is discretized into cells, labelled with a value denoting the probability for that portion of space to be occupied. From the sensor data, the robot creates a local grid, related to its current pose. A search through a set of previously acquired certainty grids is performed to find the best match. The configuration of the robot giving the highest

correlation is chosen as the robot pose estimate. Once the vehicle is localized, the local grid is merged into the global one, reducing its uncertainty. In [Thrun et al., 1998] a probabilistic grid of the environment is build, supposing that the robot occasionally observes a small number of landmarks (such as hallway intersections, corners, dead ends). The SLAM problem is posed as a maximum likelihood estimation problem, where both the landmark locations and the robot pose have to be estimated. By employing suitable probabilistic models for the robot motion and the sensory system, the likelihood of the robot and landmark positions is maximized using the Baum-Welch algorithm. This techniques are particularly appealing for their simplicity and easiness of implementation. A successful application of this approach to the autonomous navigation of a robot acting as a museum tour-guide, has been presented in [Thrun et al., 2000b].

The main drawback of the grid-based approach relies on its intrinsically high computational burden and storage requirement. Moreover, the effectiveness of those methods strongly depends on the accuracy of the sensory system’s model, since misinterpreted noisy data may lead to inconsistent map management.

2.1.2b Topological Approach

Driven by the observation that human beings are able to successfully navigate, in spite of the poor accuracy of their sensory systems and in presence of frequently incomplete knowledge, topological techniques adopt a qualitative description of the surrounding environment, rather than building a metrically accurate map. The environment is represented by a set of distinctive landmarks and places. In [Kuipers and Byun, 1991], a hierarchical description of the spatial environment, is proposed.

Distinctive places and paths are defined by their properties at the control level, and serve as the nodes and arcs of the topological model. During the exploration, place matching is done using global topological constraints as well as local metrical comparison, in order to discriminate new places from already visited ones. [Choset and Nagatani, 2001] presented a method that exploits the topology of the robot’s free space, embodied into a generalized Voronoi graph (GVG), which encodes some metric information about the robot’s environment as well. [Mataric, 1992] proposes a behavior-based control scheme, which constructs a topological map of the environment from sonar and compass data

In general, those methods are appropriate for navigation in simple environments and for creating models of human and animal cognition. However, for many operations, metrically accurate maps, that are globally referenced, are necessary in order to accomplish the goal of the mission.

2.1.2c Feature-Based Approach

In the feature-based approach, the environment is mapped in terms of a set of remarkable features (landmarks), whose positions have to be estimated, together with the robot pose. Feature-based maps can explicitly store a set of points, lines, features or etc. that may be perceived by a mobile robot, along with their position. The first attempts to phrase the SLAM problem in an estimation-theoretic framework can be found in the pioneering words by [Smith and Cheeseman, 1986] and [Durrant-Whyte, 1988], who posed the statistical basis of environment representation. At the same time, [Chatila and Laumond, 1985], [Moutarlier and Chatila, 1989] and [Ayache and Faugeras,

1989] started to address the problem of visual navigation via Kalman filtering. As noticed later [Smith et al., 1990], those trends of research had much in common, all showing that the estimation errors of landmark locations are necessarily correlated, since they are based on relative measurements taken from the same uncertain positions (the robot pose). As recently proved by [Dissanayake et al., 2001] for the linear case, in the limit, the landmark estimates are fully correlated. Therefore, maintaining the full state covariance matrix is of paramount importance, as far as map consistency is concerned. As a matter of fact, simply neglecting landmark correlation leads to overoptimistic estimates, as shown in [Uhlmann et al., 1997] and [Castellanos et al., 1997]. Unfortunately, when exploring large areas, densely populated of features, the computational burden required by the update of the full covariance matrix can rapidly become too cumbersome for real-time operations. For instance, if n landmarks have to be estimated, the storage requirements of the EKF grows as n² and the computational complexity of the measurement update is O(n³), although it can be reduced to O(n²) exploiting the specific structure of the SLAM problem. The cost involved in the management of the full state covariance matrix circumscribes the application of such techniques to middle-sized environments (map-scaling problem).

Most recent works are aimed at finding suboptimal solutions, trade off between computational complexity and estimation accuracy. The Covariance Intersection method, proposed by [Uhlmann et al, 1997], is able to perform the map update in constant time, making worst-case assumptions about the correlation (i.e. full correlation) among the state variables, while preserving the consistency of the estimates. Unfortunately, this approach may lead to unnecessary conservative estimates, with slow rate of