Kalman Filter Based. Localization

Transcription

1 Autonomous Mobile Robots Localization "Position" Global Map Cognition Environment Model Local Map Path Perception Real World Environment Motion Control Kalman Filter Based Localization & SLAM Zürich Autonomous Systems Lab

2 2 Kalman Filter Localization

3 3 Introduction to Kalman Filter () Two measurements Weighted least-square Finding minimum error After some calculation and rearrangements

4 4 Introduction to Kalman Filter (2) In Kalman Filter notation

5 5 Introduction to Kalman Filter (3) Dynamic Prediction (robot moving) st hour, Motion u = velocity w = noise Combining fusion and dynamic prediction

6 6 The Five Steps for Map-Based Localization Encoder Prediction Measurement and Position position estimate Estimation (odometry) (fusion) Map data base ures ns dicted feat bservation Pred o YES Matching matched predictions and observations raw sensor data or. Prediction based on previous estimate and odometry extracted features 2. Observation with on-board sensors 3. Measurement prediction based on prediction and map Observation on-board sensors 4. Matching of observation and map 5. Estimation -> position update (posteriori position) perce eption

7 5 7 Kalman Filter for Mobile Robot Localization: Robot Position Prediction In a first step, the robots position at time step + is predicted based on its old location (time step ) and its movement due to the control input u(): p ˆ( ) = f ( pˆ( ), u( )) + f: Odometry function T Σ p ( + ) = p f Σ p ( ) p f + u f Σ u ( ) u f T

8 5 8 Kalman Filter for Mobile Robot Localization: Robot Position Prediction: Example s xˆ( ) + s pˆ ( + ) = pˆ( ) + u( ) = yˆ( ) θ ˆ( ) p p p p T + sl sr sl cos( θ+ 2 2b + s l sr s l sin( θ+ 2 2b sr sl b r ) r ) Σ ( + ) = f Σ ( ) f + f Σ ( ) u u u f T Odometry Σ u ( ) = r s 0 r l 0 s l

9 9 Kalman Filter for Mobile Robot Localization: Observation The second step it to obtain the observation Z(+) ( )(measurements) from the robot s sensors at the new location at time + The observation usually consists of a set n 0 of single observations z j (+) extracted from the different sensors signals. It can represent raw data scans as well as features lie lines, doors or any ind of landmars. The parameters of the targets are usually observed in the sensor frame {S}. Therefore the observations have to be transformed to the world frame {W} or the measurement prediction have to be transformed to the sensor frame {S}. This transformation is specified in the function h i (see later).

10 0 Kalman Filter for Mobile Robot Localization: Observation: Example Raw Date of Laser Scanner Extracted Lines Extracted Lines in Model Space line j αj r j j R Sensor j ( + = (robot) frame z ) α rj σαα σα r ΣR, j( + ) = σrα σrr j

11 5 Kalman Filter for Mobile Robot Localization: Measurement Prediction ( + ) pˆ = In the next step we use the predicted robot position and the map M() to generate multiple predicted observations z t. They have to be transformed into the sensor frame ẑ ( + ) = h ( z,pˆ p ( + ) i i t We can now define the measurement prediction as the set containing all n i predicted observations Ẑ ( + ) = ẑ ( + )( i ) { } i n i The function h i is mainly the coordinate transformation between the world frame and the sensor frame

12 5 2 Kalman Filter for Mobile Robot Localization: Measurement Prediction: Example For prediction, only the walls that are in the field of view of the robot are selected.

13 3 Kalman Filter for Mobile Robot Localization: Matching: Example

14 5 4 Kalman Filter for Mobile Robot Localization: Estimation: Applying the Kalman Filter Kalman filter gain: Update of robot s position estimate: The associate variance

15 5 Kalman Filter for Mobile Robot Localization: Estimation: D Case For the one-dimensional case with we can show that the estimation corresponds to the Kalman filter for one-dimension presented earlier.

16 6 Kalman Filter for Mobile Robot Localization: Estimation: Example Kalman filter estimation of the new robot position : pˆ ( ) ) By fusing the prediction of robot position (magenta) with the innovation gained by the measurements (green) we get the updated estimate of the robot position (red)

17 7 Autonomous Indoor Navigation (Pygmalion EPFL) very robust on-the-fly localization one of the first systems with probabilistic sensor fusion 47 steps,78 meter length, realistic office environment, conducted 6 times > m overall distance partially difficult surfaces (laser), partially few vertical edges (vision)

18 8 Videos videos (Pygmalion and Recsys)

19 9 Summary of the EKF Localization EKF = Extended Kalman Filter The ingredients of the EKF Localization problem are: Prediction of the next robot position X based on the previous position X- and the odometry input u- X f X, u ) [ X = N X, P ) ] = ( ( Statistical error model of the odometry is Gaussian with: Statistical observation model is Gaussian: The map is given! [ u = N( u, Q )] [ z = N h( x ), R )] ( EKF Equations: PREDICTION X = f ( X, u ) P = F x PF T x ESTIMATION T T X = X + P H [ H P H + R ][ z h ( X ) ] + F QF u T u P + + = P P H T [ ] T T H P H + R H P

20 5 20 Summary of the EKF Localization: Prediction Phase What is the state X? X = (X,Y,θ) What s the odometry input u and its covariance matrix Q? = L R s s Q α α 0 0 What s the odometry input u and its covariance matrix Q? u = ), ( L R S S α s 0 What is f? ), ( = u X f X s s s s b s s s s Y X Y X L R L R L R L R sin 2 cos 2 θ θ + + = b s s b Y Y L R 2 sin 2 θ θ θ

21 5 2 Summary of the EKF Localization: Observation Phase Let s assume that the map is made of points with world coordinates m i = ( xi, yi ) Let s assume that the robot uses a laser range finder which measures the range r and the bearing θ of each point in the robot frame. i i r The observation z is then z = i ϕ m 2 = ( x2, y2) ) And the predicted observation z = h(x ) m = x, ) m i = ( xi, yi ) ) i ) i r z = = h ( X ) i ϕ = 2 2 ( x X ) + ( y Y ) i y arctan xi i i Y X ( y ϕ ϕ i θ

22 22 Summary of the EKF Localization: Innovation The observation (magenta): z ) z = h(x) The predicted observation (green) = what the robot would observe if its position was the one estimated in the prediction phase ) z z The difference is called Innovation

23 23 Summary of the EKF Localization: Estimation During he Estimation phase the robot position and uncertainty are updated by fusing the information of the prediction phase with that from the observation (red)

24 24 Autonomous Map Building (SLAM) Starting from an arbitrary initial point, a mobile robot should be able to autonomously explore the environment with its on board sensors, gain nowledge about it, interpret the scene, build an appropriate map and localize itself relative to this map. SLAM The Simultaneous Localization and Mapping Problem

25 25 References for interested people Simultaneous Localization and Mapping: Part I by HUGH DURRANT- WHYTE and TIM BAILEY, Robotics and Automation Magazine, June, 2006 Simultaneous Localization and Mapping: Part II by TIM BAILEY and HUGH DURRANT-WHYTE, Robotics and Automation Magazine, September 2006

26 26 Map Building: How to Establish a Map. By Hand Automatically: Map Building The robot learns its environment Motivation: - by hand: hard and costly - dynamically changing environment - different loo due to different perception

27 27 Map Building: The Problems. Map Maintaining: Keeping trac of changes in the environment 2. Representation and Reduction of Uncertainty e.g. disappearing cupboard position of robot -> position of wall? position of wall -> position of robot - i.e. measure of belief of each environment feature probability densities for feature positions inconsistent map due to motion drift

28 28 Overview of the SLAM problem Overview Simultaneous localization and mapping (SLAM) is a technique used by robots and autonomous vehicles to build up a map within an unnown environment while at the same time eeping trac of their current position. This is not as straightforward as it might sound due to inherent uncertainties ti in discerning i the robot's relative movement from its various sensors. If at the next iteration of map building the measured distance and direction traveled has a slight inaccuracy, then any features being added to the map will contain corresponding errors. If uncheced, these positional errors build cumulatively, grossly distorting the map and therefore the robot's ability to now its precise location. There are various techniques to compensate for this such as recognizing i features that t it has come across previously and re-sewing recent parts of the map to mae sure the two instances of that feature become one. Some of the statistical techniques used in SLAM include Kalman filters, particle filters and matching of laser scan data or visual landmars. Sensors characteristics A sensor is characterized principally by:. Noise 2. Dimensionality of input a. Planar laser range finder (2D points) b. 3D laser range finder (3D point cloud) c. Camera features (2D coordinates (pixels) if single camera or 3D coordinates if stereo camera) 3. Frame of reference a. Laser/camera in robot frame b. GPS in earth coord. Frame c. Accelerometer/Gyros in inertial coord. frame

29 29 SLAM overview

30 30

31 3

32 32

33 33

34 34

35 EKF based SLAM 5 35 The framewor is the same we have seen in the localization problem and can be solved by introducing an extended state X! LOCALIZATION The state X is the ONLY robot SLAM The state X is the robot configuration The state X is the ONLY robot configuration, that is: X = (X,Y,θ) g (X,Y,θ) PLUS the features m i =(x i,y i ) in the map, that is: X = (X,Y,θ,x,y,,x 2,y 2,,x i,y i ) And the prediction function was And the Prediction Phase function is X f X ) ( + + b s s s s L R L R 2 cos 2 θ ), ( = u X f X = y x u X f y x X M M ), ( = b s s b s s s s b Y X Y X L R L R L R 2 sin θ θ θ i i i i y x y x M M

36 EKF based SLAM: Estimation Phase 5 36 The observation model is the same as the EKF localization: ( ) ( ) + = = = i i i i i i X x Y y Y y X x X h r z arctan ) ( 2 2 ϕ ) ) ), ( y x m = ) ( i x i X In principle, the estimation can now proceed in the same manner as a ), ( y x m = ), ( i i y xi m = proceed in the same manner as a conventional EKF. ϕ X θ ϕ i = = i y x m X X M M = O M M M L L L m m m m X m m m m m Xm Xm Xm XX P P P P P P P P P P i i i y x m O M M M

37 37 Summary of the EKF SLAM EKF = Extended Kalman Filter The ingredients of the EKF Localization problem are: Prediction of the next robot position X based on the previous position X- and the odometry input u- X = ( X, u ) X = N X P f [ (, )] [ u = N( u, Q )] [ z = N h( x ), R )] Statistical error model of the odometry is Gaussian with: Statistical observation model is Gaussian: The map is now given and is part of the state to X! ( EKF Equations: PREDICTION X = f ( X, u ) P = F x PF T x ESTIMATION T T X = X + P H [ H P H + R ][ z h ( X ) ] + F QF u T u P + + = P P H T [ ] T T H P H + R H P

38 4d 38 EKF based Based SLAM (ETH - ASL) 4d - Perception - Features corner features extracted from lines connecting corners to structures

39 5 39 Particle Filter SLAM (FastSLAM, Montemerlo 2002) (/2) FastSLAM approach It solves the SLAM problem using particle filters. Particle filters are mathematical models that represent probability distribution as a set of discrete particles which occupy the state space. Particle filter update In the update step, a new particle distribution is generated given the motion model and the controls applied. a) For each particle:. Compare particle s prediction of measurements with actual measurements 2. Particles whose predictions match the measurements are given a high weight b) Filter resample: Resample particles based on weight Filter resample Assign each particle a weight depending on how well its estimate of the state agrees with the measurements and randomly draw particles from previous distribution based on weights creating a new distribution.

40 5 40 Particle Filter SLAM (FastSLAM, Montemerlo 2002) (2/2) FastSLAM approach (continuation) Particle set The robot posterior is solved using a Rao Blacwellized particle filtering using landmars. Each landmar estimation is represented by a 2x2 EKF (Extended Kalman Filter). Each particle is independent (due the factorization) from the others and maintains the estimate of M landmar positions.

41 4 Cyclic Environments Small local error accumulate to arbitrary large global errors! This is usually irrelevant for navigation However, when closing loops, global error does matter Courtesy of Sebastian Thrun

42 42 Probabilistic Feature Based 3D Maps (ETH-ASL) raw 3D scan of the same scene raw data photo of the scene decompose space into grid cells fill cells with data find a plane for every cell using RANSAC one plane per grid cell fuse similar neighboring planes together segmented planar segments final segmentation

43 4d - Perception - Features 5 43 Example Result photograph of corridor at ASL raw 3D scan plane segmentation result extracted planes for every cube

44 44 Probabilistic Plane Based 3D SLAM (EKF based) close-up of a reconstructed hallway close-up of reconstructed booshelves The same experiment as before but this time planar segments are visualized and integrated into the estimation process

45 45 Dynamic Environments Dynamical changes require continuous mapping If extraction of high-level features would be possible, the mapping in dynamic environments would become significantly more straightforward. e.g. difference between human and wall Environment modeling is a ey factor for robustness?

46 46 Key Problems in SLAM One of the first ey problem of SLAM is the recognition of a place already visited In order to reduce the drift the robot should detect loops in the enviroments This tass can be solved by detecting distinctive features from the environments. Laser scan features can lead however to ambiguous situations (T junctions in corridors, etc.) In the last 5 years place recognition (useful for loop detection) algorithms using cameras have been developed, d which h use distinctive visual features (e.g. SIFT features, Harris, features, etc.). Indeed, images offer a much larger quantity of information compared to 2D and 3D laser. Also SLAM algorithms have been developed d which h only use a single camera! See next slides

47 Autonomous Mobile Robots Real Time 3D Visual SLAM with a Single Camera This presentation is based on the following papers: Andrew J. Davison, Real-Time Simultaneous Localization and Mapping with a Single Camera, ICCV 2003 Nicholas Molton, Andrew J. Davison and Ian Reid, Locally Planar Patch Features for Real-Time Structure from Motion, BMVC 2004 Zürich Autonomous Systems Lab

48 48 Structure From Motion (SFM) Structure from Motion: Tae some images of the object to reconstruct t Features (points, lines, ) are extracted from all frames and matched among them All images are processed simultaneously Both camera motion and 3D structure can be recovered by optimally fusing the overall information, up to a scale factor Robust solution but far from being Real- Time!

49 49 Visual SLAM (Video From Davison 03) It is Real-Time!

50 50 State of the System The State vector and the first-order uncertainty are: Where: Where r is the camera position r = (X,Y,Z) and q is the camera orientation which is represented using quatternion (4-element vector). v and w are the translational and rotational velocity! In the SLAM the Covariance matrix is not diagonal, that is, the uncertainty of any feature affects the position estimate of all other features and the camera

51 5 EKF based Visual SLAM The state vector X and the Covariance matrix P are updated during camera motion using an EKF: Prediction: a motion model is used to predict where the camera will be in the next time step (P increases) X = f ( X, u ) P = F PF + x T x F u QF Observation: a new feature is measured (P decreases) X + = X + P H T T T u [ T H P H + R][ z h( X )] [ + ] T T H P H R H P P = P P H + +?

52 52 Prediction Phase (/2): A Motion Model for Smoothly Moving Camera Prediction Phase Attempts to predict where the camera will be in the next time step In the case the camera is attached to a person, the unnown intentions of the person can be statistically modeled Davison uses a Constant Velocity Model!

53 53 Prediction Phase (2/2): Constant Velocity Model The unnown intentions, and so unnown accelerations, are taen into account by modeling the acceleration as a process of zero mean and Gaussian distribution: By setting the covariance matrix of n to small or large values, we define the smoothness or rapidity of the motion we expect. In practice these values were used: 4 m / s 6 rad / s

54 54 Localization: Predicted Observation (/2) Predicted Observation By predicting the next camera pose, we can predict where each features is going g liely to appear: z ) i u = = h( X ) v = where And the uncertainty of the predicted observation is:

55 55 Localization: Predicted Observation (2/2) At each frame, the features occurring at previous step are searched in the elliptic region where they are expected to be according to the motion model (Normalized Sum of Square Differences is used for matching) Once the features are matched, the entire state of the system is updated according to EKF

56 56 SLAM: Adding new features in the map The feature is firstly initialized as a 3D line Along this line, 00 possible feature positions are set uniformly in the range m A each time, each hypothesis is tested by projecting it into the image Each matching produces a lielihood for each hypothesis and their probabilities are recomputed

57 57 Improved Feature Matching Up to now, traced features were treated as 2D templates in image space Long-term tracing can be improved by approximating the feature as a locally planar region on 3D world surfaces

58 58 Locally Planar 3D Patch Features

59 59 References A. J. Davison, Real-Time Simultaneous Localization and Mapping with a Single Camera, ICCV 2003 N. Molton, A. J. Davison and I. Reid, Locally Planar Patch Features for Real-Time Structure from Motion, BMVC 2004 A. J. Davison, Y. Gonzalez Cid, N. Kita, Real-Time 3D SLAM with wide-angle vision, IFAC Symposium on Intelligent Autonomous Vehicles, 2004 J. Shi and C. Tomasi, Good features to trac, CVPR, 994.

60 Autonomous Mobile Robots Applications: i Current Wor on Visual 3D SLAM done at the ETH-ASL Zürich Autonomous Systems Lab

61 Omnidirectional Visual Odometry Assumptions Method 2D Planar Motion Calibrated omnidirectional camera SIFT and homography for the translation [Triggs 98] Appearance based method for rotation. [Scaramuzza, 2008] IEEE Trans. On Robotics, October [Scaramuzza, 2008] ECCV - OMNIVIS 08

62 Ground Tracing View View 2 h n Image coordinates of two different views of a plane are related by a Homography H λ x2 = H x T n h T For a calibrated camera, motion can be computed from H using the = Triggs algorithm [Triggs 98] H R +

63 Visual Odometry Results Error at the loop closure: 6.5 m Error of orientation: 5 deg Trajectory length: 400 m

64 Loop Detection and Closing

65 Visual Odometry Results Before Loop Closure Error at the loop closure: 6.5 m Error of orientation: 5 deg Trajectory length: 400 m After Loop Closure

66 SMART trajectory on Google Earth drawn manually from the images (NOT GPS)

67

68

69 Our 3D Models built from Point Clouds and Images Lowenstrasse, Zurich Scaramuzza, 2009

70 70 Current open student projects The previous slides show some of our current research in visual odometry, SLAM, and 3D modeling of city environments. If you are a motivated student looing for a Master, Bachelor, or Semester project contact Davide Scaramuzza