Robot Localisation. Henrik I. Christensen. January 12, 2007

Transcription

1 Robot Henrik I. Robotics and Intelligent GT College of Computing Georgia Institute of Technology Atlanta, GA hic@cc.gatech.edu January 12, 2007

2 The Robot Structure

3 Outline Sum of 5 6

4 A fundamental part of mobile robotics Two problems: 1 Pose initialisation / Kidnapped Robot Initialising the system / recovery 2 Pose Maintenance pose as the robot moves about

5 Pieces for the puzzle A model for the robot Model of the environment Representation of environment A method for feature extraction A strategy to match features to the model A method to update the pose estimate

6 The basic pieces in localisation Update of map & pose estimate Pose estimate Corrected pose est World Model of pose & map features

7 The issues involves Managing the uncertainty for the pose estimate 1 vs Many hypotheses for pose Selecting a model for the environment Modelling of the system Efficient Implementation(s)

8 Approach Uncertainty in vehicle model the pose estimate Selection of different models for uncertainty

9 Outline Sum of 5 6

10 Representation Already covered in lecture on sensors and features A number of different options: 1 Topological maps 2 Feature maps 3 Probability Grid 4 Appearance based/raw models

11 Topological Map Example C8 R6 C14 R5 C7 C6 R15 R4 C5 R3 C4 C13 R2 R1 C10 C3 C2 C1 C9 R12 R11 C12 R7 R8 C11 R10 R13 R9 R14

12 Metric Maps: Grids and Features

13 Appearance / Raw Sensor Map Example

14 Outline Sum of 5 6

15 of vehicle motion Consider the process as sense-move-sense-... As part of the movement step there is a need to estimate the new pose of the robot and the associated uncertainty in the position. is based entirely on odometric information and a model of the robot as discussed in kinematic modelling. Uncertainty is modelled by covariance propagation as discussed in under sensing

16 Model for differential drive robot Y I Y R P X R θ X I Assume a pose estimate of x p = y θ A motion of left and right wheel by s l and s r respectively. The distance between the wheel is assumed to be 2l

17 Pose prediction Differential drive robot Consequently: s = s l + s r 2 θ = s r s l 2l ( x = s cos θ + θ ) 2 ( y = s sin θ + θ ) 2

18 Pose prediction Differential drive robot Or in condensed form: x p = y + θ s cos ( θ + θ ) 2 s sin ( θ + θ ) s r s l 2l 2

19 Pose Uncertainty estimate Need to provide an estimate of uncertainty in position Σ p Assume the initial uncertainty is Σ p Assume motion uncertainty is [ ] kr s Σ = r 0 0 k l s l Assumption of uncertainty, and proportional to distance travelled. k i is determined by calibration

20 Pose prediction Uncertainty estimate Update can be generate by covariance propagation, i.e.: Σ p = p f Σ p p f T + rl f Σ rl f T Where p f and rl f is the Jacobians for the pose and the motion, respectively.

21 Pose Pose/Motion Jacobian p f = [ f x f y cos θ + θ 2 rl = sin θ + θ 2 f θ s 4l s 4l 1 4l ] 1 0 s sin ( θ + θ ) 2 = 0 1 s cos ( θ + θ ) sin θ + θ 2 cos θ + θ cos θ + θ sin θ + θ 2 + s 4l + s 4l 1 4l sin θ + θ 3 2 cos θ + θ 7 2 5

22 Pose prediction Linear Motion Example

23 Pose prediction Circular Motion Example

24 Outline Sum of 5 6

25 Pose Given an estimate of where the robot might be p Acquire sensor data Extract Features Match features to model Update the pose estimate based on feature information

26 Pose updating Uncertainty Model The selection of an uncertainty model Single hypothesis s Probability grid Topological Graph Particle Based

27 Pose updating - Uncertainty Model The selection of an uncertainty model influences the updating methodology The uncertainty model is coupled to the environmental representation The model influences strongly the computational requirements

28 Uncertainty Modelling Markov Approach Assume the world is divided into places/states s P of p(s t ) given s t 1 and sensory data z t Formally p(s t z t ) = p(s t s t 1, z t )p(s t 1 )ds t 1 Integration needed as s t could be reached from multiple locations

29 Uncertainty modelling Markov Approach Markov assumption: all knowledge encoded in the pose/state estimate There is a probability model for motion updating There is a model for p(z s) i.e. a sensor model, as p(s z) = p(z s)p(s) p(z) where p(s) is location model and p(z) is the sensor noise model These assumptions are relative weak

30 Topological modelling dervish example

31 Topological modelling dervish example Here the probability updating is used for direct lookup of p(s z), where s is any of the nodes in the topological map As robot moved through environment the graph is updated with new information The probability table is small and efficient to handle The localisation is coarse (location oriented)

32 Pose estimation with Gaussian Model The pose is approximated by a single Gaussian function p(s) = ( ) exp (s s)σ 1 s (s s) T 2πΣs 2 s is here a continuous function and Σ s is the associated uncertainty estimate is normally performed using a Kalman filter model

33 Kalman filter State space model s t = Fs t 1 + Gu t + w t z t = Hs t + v t where F is the system model, G is the deterministic input, H is a prediction of where features are in the world, w is the system noise, and v is the measurement noise

34 Detour Probability Assume two measurement x 1 and x 2 with associated uncertainties σ 1 and σ 2. How does one generate an optimal estimate ˆx? Doing a weighted least square S = w i (ˆx x i ) 2 i=1,2 what are the optimal weights w i? From S ˆx = 0 we get...

35 Detour Probability ˆx = wi q i wi with w i = 1 σ 2 i and we get ˆx = σ2 2 σ1 2 + x 1 + σ2 1 σ2 2 σ1 2 + x 2 σ2 2 σˆx = σ2 1 σ2 2 σ σ2 2

36 Detour Probability The update can be rewritten to ˆx = x 1 + σ2 1 σ1 2 + (x 2 x 1 ) σ2 2 I.e. the updating = the value + a correction term

37 The Kalman model Composed of a prediction and an update of the estimate is the same as kinematic prediction of pose Update is based on the sensory feedback

38 Kalman s t t 1 = Fs t 1 t 1 + Gu t Σ t t 1 = F Σ t 1 t 1 F T + Q t where Q is the uncertainty of the kinematic motion (odometric uncertainty)

39 Kalman s t t = s t t 1 + K t (z t Hs t t 1 ) K t = Σ t t 1 H T S 1 t S t = HΣ t t 1 H T + R t Σ t t = (I K t H)Σ t t 1

40 Kalman interpretation F is the kinematic update H is the measurement prediction p(z s) ie a prediction of where features in the world are in the sensory frame Σ is the uncertainty in the pose/state estimate s t t. S t is the uncertainty in the sensory measurements R t is the sensor noise

41 Use of the Kalman filter Sensor Data Feature Extraction Matching Odometric Data / Update as described above Matching typically based on the Mahalanobis distance M = z t St 1 zt T A validation gate may be used: M < ρ Model/Pose

42 Kalman Example Kinematic prediction of position and uncertainty Standard model

43 Kalman Example Features

44 Kalman Example Matching using nearest neighbour and predicted features matching is easy

45 Kalman Example Matched features generates an error in estimates (purple - model), (green - measured), and (red - update) is now trivial

46 Kalman Discussion Makes an assumption of a single pose estimate By far the most frequently used model Easy to compute. of F and H can be difficult. For non-linear systems that are Jacobians and must be computed for each step.

47 s Sometimes there might be multiple interpretations of the feature matches Each interpretation generates a hypothesis for the robot pose Multiple Distributions are generated and a number of them are used in different Kalman updates. When a model receives no matches or the uncertainty grows too large, i.e. trace(σ) > δ then the model is terminated. Can be computationally challenging

48 Sum of Gauss/Hypothesis Door Detection

49 Sum of Gauss/Hypothesis More Evidence

50 Space can be tessellated and the update can be performed directly in pose space p(s) = α s P p(s )p(s s, o) where α is a normalising factor

51 Grid updating during motion

52 Grid updating example

53 Probability grids For accurate localisation in pose space the process can become challenging Say, a m space with a resolution of 10 cm and an angular resolution of 0.1 deg the space is or 3.6 GB of data that must be updated in each time step. Various approximation (windowing) methods can be used.

54 Based based methods is using a sample model for approximation of the pose estimate Using a grid model as presented earlier Assume with have a number of particles in a collection { } S t =, π (i) ) i = 1..N (s (i) t each particle is a hypothesis for the position of the robot, and π (i) t is an associated weight We can now approximate p(s t z 0, z 1,...z t ) for any distribution of the pose hypotheses t

55 Strategy 1 Draw N samples from an initial. Typically a uniform distribution. Give each sample a weight of 1 N 2 Propagate the motion information and draw a new sample from the distribution p(s (i) t+1 s(i) t, o t ) 3 Set the weight of the sample to π (i) t+1 = p(z t+1 s (i) t+1 ) π(i) t based on sensory input 4 Generate a new sample set by drawing samples from the current set and a basis distribution (typically uniform). Normalize the weights 5 Go back to step 2

56 Example Example of particle distribution about estimate of position Sonar readings for update of the position Video of system in operation

57 Example Burgard, Fox & Thrun

58 Discussion Efficient to approximate any distribution of the pose The number of particles can be adopted to a particular platform Can be used both for simple and multi robot localisation

59 Outline Sum of 5 6

60 / Mapping Example Now mapping and localisation is also integrated to allow for autonomous operation in general environments The mapping and localisation can be integrated to generate Simultaneous and Mapping (SLAM) Indoor example VIDEO Outdoor example VIDEO

61 Outline Sum of 5 6

62 Wrap-Up is a fundamental competence in mobile robotics Involves two major steps of motion (kinematic modelling) of pose estimate(s) The method used depends upon the adopted model for handling of uncertainty and the associated world model Brief introduction to the main methods for estimation A few illustrative examples