BEST2015 Autonomous Mobile Robots Lecture 3: Mobile Robot Localization

Transcription

1 BEST2015 Autonomous Mobile Robots Lecture 3: Mobile Robot Localization Renaud Ronsse École polytechnique de Louvain, UCLouvain July Introduction c Siegwart et al., 2011, Fig. 6.27, p. 417 Localization is about determining the robot s position within the environment. 2

2 Do we need to localize or not? To go from A to B, does the robot need to know where it is? c Siegwart et al., 2011, Fig. 5.6, p. 276 Navigation without hitting obstacles + detection of goal location Possible by following always the left wall, but how to detect that the goal is reached? What about efficiency? 3 1 Introduction 2 The Challenge of Localization 3 Odometry 4 Belief Representation 5 Probabilistic Map-Based Localization 6 Kalman Filter Localization 7 References 4

3 Global localization requires external devices Example 1: GPS... but this is not accurate! c Siegwart et al., 2011, ETH-Z lecture slides Cannot function indoor and in obstructed areas. 5 Global localization requires external devices Example 2: Beacon Based Localization Systems c Siegwart et al., 2011, Fig. 5.41, p. 347 Ex 1: Poles with highly reflective surface and a laser for detecting them Ex 2: Colored beacons and an omnidirectional camera for detecting them Advanced hardware! 6

4 Global localization requires external devices Example 3: Motion Capture Systems c Siegwart et al., 2011, ETH-Z lecture slides High resolution (from VGA up to 16 Mpixels) Very high frame rate (several hundreds of Hz) Good for ground truth reference and multi-robot control strategies Popular brands: VICON, OptoTrak Very expensive! 7 Beacon Based Localization Systems (1/2) Objective: localize the robot w.r.t. the beacon... or the beacon w.r.t. the robot! BEACON rotating laser ROBOT What is the orientation θ and the distance d? θ = θ 1 + θ 2 2 d = R sin β sin α 2 = R ( sin β ) sin θ2 θ 1 2 R sin ( θ2 θ 1 2 ) 8

5 Beacon Based Localization Systems (1/2) Then, absolute localization can be obtained by triangulation, with at least 3 beacons whose absolute localization is known. c Pierlot and Van Droogenbroeck, IEEE T-RO 2013 Actually, only the angles θ are necessary... 9 Beacon Based Localization Systems (2/2) KIVA Systems, Boston (MA) Warehouse Robots at Work Youtube Unique marker with known absolute 2D position in the map. 10

6 Noise and Aliasing Perception and motion play an important role in localization. Sensor noise: Limitation on the consistency of sensor readings in the same environmental state. Examples: effect of luminosity (e.g. sun) on color-based vision sensors; changes in material reflexion properties for sonars; interferences;... Solution? Temporal and/or multi-sensory fusion. Sensor aliasing: Nonuniqueness of sensor readings: several environment states trigger the same sensory readings! Effector noise: A single action taken by a mobile robot may have several different possible results. Error in motion is viewed as an error in odometry. The source of error generally lies in an incomplete model of the environment: slopes, slip, (human) perturbations, etc. 11 Odometry Position update based on proprioceptive sensors (i.e. no external devices). Position error accumulates over time. Major Error Sources in Odometry: Limited resolution during integration (time increments, measurement resolution) Misalignment of the wheels (deterministic) Unequal wheel diameter (deterministic) Variation in the contact point of the wheel Unequal floor contact (slipping, not planar,... ) deterministic (systematic) non-deterministic (non-systematic) Deterministic errors can be eliminated by proper calibration of the system. Non-deterministic errors have to be described by error models and will always lead to uncertain position estimate. 12

7 Odometry of differential-drive vehicle c Siegwart et al., 2011, Fig. 5.3, p. 271 Assuming rolling without sliding: ω(r + d) = v l ω(r d) = v r c Siciliano and Khatib (Eds.), 2008, Fig. 20.1, p. 477 such that V = ωr = v r+v l and 2 x(t) = V (t) cos (θ(t))dt; y(t) = V (t) sin (θ(t))dt; θ(t) = ω(t)dt 13 Error model for odometric position estimation Incremental travel distances: x p = y = p + p = θ = x y θ x y θ + + s cos (θ + θ/2) s sin (θ + θ/2) θ s r + s l ( ) 2 cos θ + s r s l ( 4b ) 2 sin θ + s r s l 4b s r + s l s r s l 2b So, p = f(x, y, θ, s r, s l ). 14

8 Error model for odometric position estimation So, p = f(x, y, θ, s r, s l ). Assuming now an error model: Σ = covar( s r, s l ) = [ kr s r 0 0 k l s l ] The two errors of the individually driven wheels are independent; The variance of the errors (left and right wheels) are proportional to the absolute value of the traveled distances. The error propagation law gives: Σ p = p f Σ p p f T + rl f Σ rl f T [ ] where p f = f x, f y, f θ and rl f = function gradients. [ f s r, ] f s l are the 15 Error model for odometric position estimation So, p = f(x, y, θ, s r, s l ). p f = rl f = = [ f x, f y, f ] θ [ ] f f, s r s l 1 2 cθ s 2b sθ 1 2 sθ + s 2b cθ ( where cθ = cos θ + θ 2 1 b ) = ( ) 1 0 s sin θ + θ ( 2) 0 1 s cos θ + θ cθ + s 2b sθ 1 2 sθ s 2b cθ 1 b ( and sθ = sin θ + θ 2 ). 16

9 Evolution of pose uncertainty straight-line movement circular movement c Siegwart et al., 2011, Figs. 5.4 and 5.5, pp Errors perpendicular to the direction of movement are growing much faster! Errors ellipse does not remain perpendicular to the direction of movement! 17 Evolution of pose uncertainty c Siegwart et al., 2011, ETH-Z lecture slides from [Fox, Thrun, Burgard, Dellaert, 2000] Note: In reality, errors are not shaped like ellipses! 18

10 1 Introduction 2 The Challenge of Localization 3 Odometry 4 Belief Representation 5 Probabilistic Map-Based Localization 6 Kalman Filter Localization 7 References 19 Belief Representation How do we represent the robot position, where the robot believes to be? a) Continuous map with single hypothesis probability distribution b) Continuous map with multiple hypotheses probability distribution c) Discretized map with probability distribution d) Discretized topological map with probability distribution c Siegwart et al., 2011, Fig. 5.9, p

11 Single-hypothesis Belief Continuous Line-Map Single-hypothesis: the robot postulates its unique position. c Siegwart et al., 2011, Fig. 5.10, p. 281 The robot represents its position as a single point in the continuous set: (x, y, θ). 21 Single-hypothesis Belief Grid and Topological Map c Siegwart et al., 2011, Fig. 5.10, p. 281 c) The position is noted with the same level of fidelity as the map cell size. d) The map is abstract and identifying the position is equivalent to identifying a single node i in the graph. Single belief = no ambiguity but challenging! 22

12 Multi-hypotheses Belief Grid-based Representation The robot tracks a possibly infinite set of positions. It is convenient to sort these possible positions by providing a preference ordering, e.g. by using a mathematical distribution (continuous or discrete). c Siegwart et al., 2011, Figs. 5.9 and 5.11, pp. 279 and 282 Multi belief = the robot can maintain uncertainty about its position. This can also be incorporated in planning (i.e. choosing the path minimizing localization uncertainty). Challenge: how to make decisions? 23 Representation of the Environment Envir. Modeling c Siegwart et al., 2011, Fig. 5.10, p. 281 Raw sensor data, e.g. laser range data, grayscale images large volume of data, low distinctiveness on the level of individual values makes use of all acquired information Low level features, e.g. line other geometric features medium volume of data, average distinctiveness filters out the useful information, still ambiguities High level features, e.g. doors, a car, the Eiffel tower low volume of data, high distinctiveness filters out the useful information, few/no ambiguities, not enough information 24

13 Map representation Often the fidelity of the position representation is bounded by the fidelity of the map: 1 The precision of the map must appropriately match the precision with which the robot needs to achieve its goals. 2 The precision of the map and the type of features represented must match the precision and data types returned by the robot s sensors. 3 The complexity of the map representation has direct impact on the computational complexity of reasoning about mapping, localization, and navigation. 25 Map representation Two approaches: Continuous representation = exact Decomposition strategies c Siegwart et al., 2011, Fig. 5.12, p. 285 c Siegwart et al., 2011, Figs. 5.14, 5.16 and 5.18, pp. 288, 290 and

14 1 Introduction 2 The Challenge of Localization 3 Odometry 4 Belief Representation 5 Probabilistic Map-Based Localization 6 Kalman Filter Localization 7 References 27 Problem statement Consider a mobile robot moving in a known environment: As it starts moving (say from a precisely known location) it can keep track of its location using wheel odometry. After a certain movement the robot will get very uncertain about its position. To keep position uncertainty from growing unbounded, the robot must localize itself in relation to its environment map. To localize, the robot uses its on-board exteroceptive sensors (e.g. ultrasonic, laser, vision sensors) to make observations of its environment. Observations lead to an estimate of the robot position which can then be fused with the odometric estimation to get the best possible update of the actual robot s position. Its uncertainty shrinks. c Siegwart et al., 2011, ETH-Z lecture slides 28

15 Illustration: Improving belief state by moving The robot is placed somewhere in the environment but is not told its location. The robot queries its sensors and finds it is next to a pillar. The robot moves one meter forward. To account for inherent noise in robot motion the new belief is smoother. The robot queries its sensors and again it finds itself next to a pillar. Finally, it updates its belief by combining this information with its previous belief. c Siegwart et al., 2011, Fig. 5.22, p Why a probabilistic approach for localization? Because the data coming from the robot sensors are affected by measurement errors, we can only compute the probability that the robot is in a given configuration. The key idea in probabilistic robotics is to represent uncertainty using probability theory: instead of giving a single best estimate of the current robot configuration, probabilistic robotics represents the robot configuration as a probability distribution over the all possible robot poses. This probability is called belief. 30

16 Examples of probability distributions Example 1: Uniform distribution the robot does not know where it is: the information about the robot configuration is null: c Siegwart et al., 2011, ETH-Z lecture slides Example 2: Multimodal distribution the robot is in the region around a or b: c Siegwart et al., 2011, ETH-Z lecture slides 31 Examples of probability distributions Example 3: Dirac distribution the robot has a probability of 1.0 (i.e. 100%) to be at a given position: c Siegwart et al., 2011, ETH-Z lecture slides δ(x) = { if x = 0 0 otherwise with + δ(x)dx = 1. 32

17 Examples of probability distributions Example 4: Gaussian distribution also called normal distribution (N(µ, σ)): c Siegwart et al., 2011, ETH-Z lecture slides p(x) = 1 σ 2π e (x µ) 2 2σ 2 where µ is the mean value and σ is the standard deviation. 33 Action and perception updates In robot localization, we distinguish two update steps: ACTION (or prediction) update: the robot moves and estimates its position through its proprioceptive sensors. The robot uncertainty grows. PERCEPTION (or measurement) update: the robot makes an observation using its exteroceptive sensors and corrects its position by combining its belief before the observation with the probability of making exactly that observation. The robot uncertainty shrinks. c Siegwart et al., 2011, Fig. 5.20, p

18 Solution to the probabilistic localization problem Solving the probabilistic localization problem consists in solving separately the Action and Perception updates. A probabilistic approach to the mobile robot localization problem is a method able to compute the probability distribution of the robot configuration during each Action and Perception step. 35 Solution to the probabilistic localization problem The ingredients are: 1 The initial probability distribution 2 The statistical error model of the proprioceptive sensors, e.g. wheel encoders: [ ] kr s Σ = covar( s r, s l ) = r 0 0 k l s l 3 The statistical error model of the exteroceptive sensors, e.g. laser, sonar, camera: c Siegwart et al., 2011, ETH-Z lecture slides 4 A map of the environment (If the map is not known a priori then the robot needs to build a map of the environment and then localizes in it. This is called SLAM, Simultaneous Localization And Mapping). 36

19 Basic concepts of probability theory Theorem of Total probability: p(x) = p(x y)p(y)dy where p(x y) = p(x,y) p(y) is the conditional or posterior probability (used for action update). Bayes rule: (used for perception update). y p(x y) = p(y x)p(x) p(y) 37 Principles of Action and Perception updates (Markov localization) Action (=prediction) update: the robot estimates its current position bel(x t ) based on the knowledge of the previous position bel(x t 1 ) and the odometric input u t (Markov assumption): bel(x t ) = p(x t u t, x t 1 )bel(x t 1 )dx t 1 = p(x t u t, x t 1 ) bel(x t 1 ) Perception (=measurement) update: the robot corrects its previous position (i.e. its former belief) by opportunely combining it with the information from its exteroceptive sensors z t : p(x t z t ) = p(z t x t )p(x t ) p(z t ) bel(x t ) = ηp(z t x t, M)bel(x t ) where η is a normalization factor which makes the probability integrate to 1. This localization algorithm is also called a Bayes filter. 38

20 Illustration of probabilistic bap based localization Initial probability distribution: p(x) t0 bel(x 0 ). Perception update: bel(x t ) = ηp(z t x t, M)bel(x t ). Action update: bel(x t ) = p(x t u t, x t 1 ) bel(x t 1 ). Next perception update: p(z t x t ). bel(x t ) = ηp(z t x t, M)bel(x t ). c Siegwart et al., 2011, Fig. 5.22, p Markov versus Kalman localization Two approaches exist to represent the probability distribution and to compute the Total Probability and Bayes Rule during the Action and Perception phases: c Siegwart et al., 2011, ETH-Z lecture slides Markov localization allows for localization starting from any unknown position and can thus recover from ambiguous situations position tracking and global localization. Kalman filter localization tracks the robot from an initially known position position tracking only. 40

21 1 Introduction 2 The Challenge of Localization 3 Odometry 4 Belief Representation 5 Probabilistic Map-Based Localization 6 Kalman Filter Localization 7 References 41 Kalman Filter Localization: principle Special case of Markov localization: the robot belief bel(x t ) is assumed to be Gaussian. Main computation: update the mean and the covariance very efficient. c Siegwart et al., 2011, Fig. 5.29, p

22 Probability theory with Gaussian noise Assuming x 1 = N(µ 1, σ1 2) and x 2 = N(µ 2, σ2 2 ) are two independent and normally distributed variables. If y = Ax 1 + Bx 2 is a linear function of x 1 and x 2, then: or µ y = Aµ 1 + Bµ 2, σ 2 y = A 2 σ B 2 σ 2 2 Σ y = AΣ 1 A T + BΣ 2 B T if x 1 and x 2 are vectors (Σ i denotes the covariance of x i ). If y = f(x 1, x 2 ) is not a linear function of of x 1 and x 2, then it is practical to consider its first-order approximation: such that: y f(µ 1, µ 2 ) + F x1 (x 1 µ 1 ) + F x2 (x 2 µ 2 ) µ y = f(µ 1, µ 2 ), Σ y = F x1 Σ 1 F T x 1 + F x2 Σ 2 F T x 2 43 Bayes rule maximum likelihood principle Assuming p 1 (q) = N(ˆq 1, σ1 2 ) (e.g. the prediction-based belief) and p 2 (q) = N(ˆq 2, σ2 2 ) (e.g. the measurement-based belief): with p 1 (q)p 2 (q) = (q ˆq 1 1 ) 2 2σ e 1 2 σ 1 2π (q ˆq 1 2 ) 2 2σ e 2 2 σ 2 2π = Ωe (q ˆq)2 2σ 2 Ω =... 1 ˆq σ1 ˆq = σ 2 1 σ σ 2 2 ˆq 2 σ 2 = σ 2 1 σ2 2 σ σ2 2 σ min(σ 1, σ 2 ): uncertainty decreases by combining two measurements. c Siegwart et al., 2011, Fig. 5.31, p

23 Application to mobile robotics c Siegwart et al., 2011, Fig. 5.32, p. 332 Fusion of the prediction update and of the measurement update: Prediction update: applying the theorem of total probability. Measurement update: applying 4 successive steps. 45 Prediction update The robot position at time t is inferred from the old localization and the control input: ˆx t = f(x t 1, u t ). Therefore: ˆP t = F x P t 1 F T x + F u Q t F T u Remember the odometry update equation! c Siegwart et al., 2011, Fig. 5.33, p

24 Measurement update 1/4: Observation The second step it to obtain the observation Z (measurements) from the robot s sensors at the new location The observation usually consists of a set n of single observations z j extracted from the different sensors signals. It represents features like lines, doors or any kind of landmarks. The parameters of the targets are usually observed in the sensor frame {S} (local to the robot). Therefore the features in the map (in the world frame {W}) have to be transformed into the sensor frame {S}. This transformation is specified in the function h j (see later). 47 Measurement update 1/4: Observation c Siegwart et al., 2011, Fig. 5.34, p. 339 Each line feature is extracted from sensors and consist of two parameters (distance and angle) and a 2 2 covariance matrix R i t. 48

25 Measurement update 2/4: Measurement prediction The predicted robot position ˆx and the map are used to generate multiple predicted observations ẑ j which have to be transformed into the sensor frame: ẑ j t = hj (ˆx t, m j ) where m j represents the j-th feature of the map. We can now define the measurement prediction as the set containing all n i predicted observations: } Ẑ = {ẑ j 1 j n i The function h j is mainly the coordinate transformation between the world frame and the sensor frame. 49 Measurement update 2/4: Measurement prediction For prediction, only the walls that are in the field of view of the robot are selected. c Siegwart et al., 2011, Figs and 5.36, pp The measurement prediction are uncertain, because the prediction of robot position is uncertain. 50

26 Measurement update 3/4: Matching Identifying all of the single observations that match specific predicted features well enough to be used during the estimation. For each measurement prediction for which an corresponding observation is found the innovation is calculated: [ ] [ ] v ij t = zt i ẑ j t = zt i h j (ˆx t, m j ) and its innovation covariance found by applying the error propagation law: Σ ij IN t = hj ˆPt h jt + R i t where Rt i represents the covariance (noise) of the actual observation zt. i The validity of the correspondence between measurement and prediction can e.g. be evaluated through the Mahalanobis distance: v ij T ( ) t Σ ij 1 IN t v ij t g 2 where g is called the validation gate. 51 Measurement update 3/4: Matching c Siegwart et al., 2011, Fig. 5.37, pp. 342 If one observation falls in multiple validation gates, the best matching candidate is selected or multiple hypotheses are tracked. Observations that do not fall in the validation gate are simply ignored for localization. 52

27 Measurement update 4/4: Estimation Applying the Bayes rule The new robot position x t (red) is obtained by fusing the prediction of robot position (magenta) with the innovation gained by the measurements (green): x t = ˆx t + K t v t P t = ˆP t K t Σ IN t K T t where K t = ˆP t H T t (Σ IN t ) 1 is the Kalman gain. c Siegwart et al., 2011, Fig. 5.38, p Markov versus Kalman: summary Both methods solve the convolution and Bayes rules for the action and the perception update respectively, but: c Siegwart et al., 2011, ETH-Z lecture slides 54

28 Summary Behavior based navigation and map based navigation are two approaches to solve the localization problem. Usually, the second one is more robust and more flexible to strategic changes. Global localization requires external devices which are usually complex to deploy and expensive. Perception and motion both bring noise to the localization challenge. Odometry is based on internal and proprioceptive sensors. The localization error accumulates over time, faster to the direction being perpendicular to movement. Belief can be represented in a continuous or discrete framework, with single or multiple hypotheses. Probabilistic map-based localization consists in fusing observation-based estimates of the robot position with the odometric estimation solving separately the Action and Perception updates. Two approaches for probabilistic map-based localization: Markov and Kalman, with their respective pros and cons. 55 References Autonomous Mobile Robots (2nd Edition) Siegwart et al.; The MIT Press, Chapter 5 56