Probabilistic Robotics Course Presentation Outline 1. Introduction 2. The Bayes Filter 3. Non Parametric Filters 4. Gausian Filters 5. EKF Map Based Localization 6. EKF Feature-based SLAM 7. EKF Pose-based SLAM 8. Advanced SLAM Concepts

Probabilistic Robotics What is Probabilistic Robotics? ‘Robotics is the science of perceiving and manipulating the physical world through computer controlled mechanical devices.’ ‘Probabilistic robotics is a relatively new approach to robotics that pays tribute to the uncertainty in robot perception and action.’ Sebastian Thrun 1 Introduction

Probabilistic Robotics Where am I? Localization Where are the amphoras? Mapping Robot Localization 1 Introduction Probabilistic Robotics course

Probabilistic Robotics {B} Y X Step: k The Uncertainty grows without bound. The vehicle gets lost Step : k+2 Step : k+1 1 Introduction Localization: Estimate the position, orientation and velocity of a vehicle Localization through Dead Reckoning:

Probabilistic Robotics Localization through Dead-Reckoning: 1 Introduction The robot position drifts

Probabilistic Robotics The Mapping Problem A conventional method for map building is incremental mapping. Position Reference given by Dead Reckoning  Map distorsion Bad maps  Poor localization 1 Introduction [Ribas 08]

Probabilistic Robotics The Mapping Problem A conventional method for map building is incremental mapping. Position Reference given by Dead Reckoning  Map distorsion Bad maps  Poor localization 1 Introduction [Ribas 08]

Probabilistic Robotics This is an old problem… Ancient Europe. The Catalan Atlas of Introduction

Probabilistic Robotics This is an old problem… 1 Introduction

Probabilistic Robotics MapRobot Pose Environment Localization Algorithms Mapping Algorithms SLAM: Simultaneous Localization And Mapping 1 Introduction

Probabilistic Robotics > Localization Through SLAM: UPDATE {B} Y X MEASUREMENT External Sensors PREDICTION Internal Sensors Step: k+1 Step: k DATA ASSOCIATION Step: k+1 1 Introduction

Probabilistic Robotics > Localization Through SLAM: 1 Introduction [Ribas 08]

Probabilistic Robotics 1.1 Problem Statement Given –Map of the environment. –Sequence of sensor measurements. Wanted –Estimate of the robot’s position. Problem classes –Position tracking (initial pose known) –Global localization (initial pose unknown) –Kidnapped robot problem (recovery) “Using sensory information to locate the robot in its environment is the most fundamental problem to providing a mobile robot with autonomous capabilities.” [Cox ’91]

Probabilistic Robotics 1.2 Problem Statement Pose: –2D: E (x,y,θ) T –3D: E (x, y, z, ϕ,θ,φ) T Environment –Static: only robot pose changes –Dynamic: the robot as well as the pose of other entities change Localization: –Pasive: Localization module only observes –Active: Robot is guided in a way that minimizes the localization error. Y X x y {E} {G} n o {E} x y z y x Z

Probabilistic Robotics Dead Reckoning: Dead Reckoning {E} {B} x y z x y z [x k y k θ k ] function getOdometry() ΔN L =ReadEncoder(LEFT); ΔN R =ReadEncoder(RIGH T); dl = 2 * π * R * ((ΔN L )/ζ) ; dr = 2 * π * R * ((ΔN R )/ζ) ; d =(dr+dl)/2; Δθ k =(dr-dl)/w; θ k = θ k-1 + Δθ k ; x k = x k-1 + (d * cos(θ k )); y k = y k-1 + (d * sin(θ k )); Return [x k y k θ k ]

Probabilistic Robotics Odometry of the differential drive [x k y k θ k ] function getOdometry() ΔN L =ReadEncoder(LEFT); ΔN R =ReadEncoder(RIGH T); dl = 2 * π * R * ((ΔN L )/ζ) ; dr = 2 * π * R * ((ΔN R )/ζ) ; d =(dr+dl)/2; Δθ k =(dr-dl)/w; θ k = θ k-1 + Δθ k ; x k = x k-1 + (d * cos(θ k )); y k = y k-1 + (d * sin(θ k )); Return [x k y k θ k ] ζ: pulses each Wheel turn R: Wheel radious

Probabilistic Robotics 6 DOF Dead Reckoning From Velocity Homogeneous Matrix How can the robot position be defined? –For an 6DOF Mobile Robot (AUV or UAV) x y {E} x y {B} z

Probabilistic Robotics 6 DOF Dead Reckoning From Velocity Position + Roll, Pitch, Yaw attitude X=(x,y,z, ϕ,θ,ψ) Homogeneous Matrix x z y x z y How can the robot position be defined? –For an 6DOF Mobile Robot (AUV or UAV) {E} x y z Final configuration

Probabilistic Robotics Position + Roll, Pitch, Yaw attitude X=(x,y,z, ϕ,θ,ψ) Homogeneous Matrix x z y x z y x z y How can the robot position be defined? –For an 6DOF Mobile Robot (AUV or UAV) z {E} x y Final configuration Yaw - Rot(z,ψ) 6 DOF Dead Reckoning From Velocity

Probabilistic Robotics 6 DOF Dead Reckoning From Velocity Position + Roll, Pitch, Yaw attitude X=(x,y,z, ϕ,θ,ψ) Homogeneous Matrix x z y x z y x z y x z y How can the robot position be defined? –For an 6DOF Mobile Robot (AUV or UAV) {E} x y z Final configuration Yaw-Rot(z,ψ) Pitch - Rot(y,θ)

Probabilistic Robotics 6 DOF Dead Reckoning From Velocity Position + Roll, Pitch, Yaw attitude X=(x,y,z, ϕ,θ,ψ) Homogeneous Matrix z Yaw-Rot(z,ψ) Pitch - Rot(y,θ) Roll-Rot(x,Φ) x z y x z y x z y x z y How can the robot position be defined? –For an 6DOF Mobile Robot (AUV or UAV) x y {E} x y z Final configuration

Probabilistic Robotics 6 DOF Dead Reckoning From Velocity How can the robot position be defined? –UUV Position & attitude E r=(x, y, z, ϕ, θ, ψ) T –Homogeneous Matrix

Probabilistic Robotics 6 DOF Dead Reckoning From Velocity Kinematics Model of an Underwater Robot –Remember Earth - fixed X Z Y DOF Position and Euler angles x y Z ϕ Θ ψ Forces and moments X Y Z K M N Lin. and ang. Vel. u (surge) v (sway) w (heave) p (roll) q (pitch) r (yaw) η relative to inertial frame υ,τ relative to fixed-body frame Body-fixed roro Y0Y0 Z0Z0 X0X0 q r p u v w {B} {E}

Probabilistic Robotics 6 DOF Dead Reckoning From Velocity Kinematics Model of an Underwater Robot –Relationship between E η and G υ Linear Velocity Angular Velocity. x x z y x z y x z y x z y {1} {2} {B} {E} Yaw-Rot(z,ψ) Pitch - Rot(y,θ) Roll-Rot(x,Φ)

Probabilistic Robotics 6 DOF Dead Reckoning From Velocity Kinematics Model of an Underwater Robot –Relationship between E η and G υ.

Probabilistic Robotics 6 DOF Dead Reckoning From Velocity Kinematics Model of an Underwater Robot Kinematics Kinematics Simulation Kinematics BvBv Inverse Kinematics BvBv. EηEη Direct KinematicsInverse Kinematics EηEη. {E} {B} x y z x y z

Probabilistic Robotics Example I: Mobile Robot Localization in a Hallway –One dimensional hallway –Indistinguishable doors –Position of doors is known (Map) –Initial position unknown –Initial heading is known –Goal: Find out where the robot is 1.3 Localization Example

Probabilistic Robotics Markov Localization Same probability of being in any x The Robot senses a door The belief over the position is updated The Robot Moves The belief over the position is updated The Robot senses a door The belief over the position is updated The Robot Moves The belief over the position is updated 1.3 Localization Example

Probabilistic Robotics u t =2 m, x t-1 =5 m u t =4 m, x t-1 =5 m u t =6 m, x t-1 =5 m utut x t-1 u t =2 m, x t-1 =3 m u t =4 m, x t-1 =3 m u t =6 m, x t-1 =3 m utut x t Robot belief of being at state x t-1 utut 1.3 Localization Example State Transition probability

Probabilistic Robotics Robot belief of being at state x t-1 utut State Transition probability u t =2 m, x t-1 =5 m Prior Belief. Prediction of state x t. Measurement probability Robot belief of being at state x t 1.3 Localization Example