Probabilistic Robotics Course Presentation Outline 1. Introduction 2. The Bayes Filter 3. Non Parametric Filters 4. Gausian Filters 5. EKF Map Based Localization

Probabilistic Robotics Course Presentation

Outline

1. Introduction2. The Bayes Filter3. Non Parametric Filters4. Gausian Filters5. EKF Map Based Localization6. EKF Feature-based SLAM7. EKF Pose-based SLAM8. 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


Where am I?

Localization

Where are theamphoras?

Mapping

Robot Localization

1 Introduction

Probabilistic Robotics course


{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:


• Localization through Dead-Reckoning:

1 Introduction

The robot position drifts


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]


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]


This is an old problem…

Ancient Europe. The Catalan Atlas of 1376.

1 Introduction

http://upload.wikimedia.org/wikipedia/commons/b/b1/Europe_Mediterranean_Catalan_Atlas.jpeg


This is an old problem…

1 Introduction


Map Robot PoseEnvironment

Localization Algorithms

Mapping Algorithms

SLAM: Simultaneous Localization And Mapping

1 Introduction


> Localization Through SLAM:

UPDATE

{B}

Y

X

MEASUREMENT

External Sensors

PREDICTION

InternalSensors

Step: k+1

Step: k

DATAASSOCIATION

Step: k+1

1 Introduction


> 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

Xx

y

{E}

{G}

n

o {E} x

yz

y x

Z


Dead Reckoning:

Dead Reckoning

{E}

{B}

x

yz

xyz[xk yk θk] function

getOdometry()ΔNL=ReadEncoder(LEFT);ΔNR=ReadEncoder(RIGHT);

dl = 2 * π * R * ((ΔNL)/ζ) ;dr = 2 * π * R * ((ΔNR)/ζ) ;d =(dr+dl)/2;

Δθk=(dr-dl)/w;θk= θk-1+ Δθk;xk= xk-1 + (d * cos(θk));yk= yk-1 + (d * sin(θk));Return [xk yk θk]

Probabilistic Robotics Odometry of the differential drive

[xk yk θk] function getOdometry()

ΔNL=ReadEncoder(LEFT);ΔNR=ReadEncoder(RIGHT);

dl = 2 * π * R * ((ΔNL)/ζ) ;dr = 2 * π * R * ((ΔNR)/ζ) ;d =(dr+dl)/2;

Δθk=(dr-dl)/w;θk= θk-1+ Δθk;xk= xk-1 + (d * cos(θk));yk= yk-1 + (d * sin(θk));Return [xk yk θk]

ζ: pulses each Wheel turnR: 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


Position + Roll, Pitch, Yaw attitude

X=(x,y,z,ϕ,θ,ψ)

Homogeneous Matrix

xz

y xz

y


{E}

x

y

z

Final configuration



X=(x,y,z,ϕ,θ,ψ)

Homogeneous Matrix

x

zy

x

zy

xz

y


z

{E}

x

y

Final configuration

Yaw-Rot(z,ψ)

6 DOF Dead Reckoning From Velocity



X=(x,y,z,ϕ,θ,ψ)

Homogeneous Matrix

x

z

y

x

z

y

xz

y

x

zy


{E}

x

y

z

Final configuration

Yaw-Rot(z,ψ)

Pitch-Rot(y,θ)



X=(x,y,z,ϕ,θ,ψ)

Homogeneous Matrix

zYaw-Rot(z,ψ)

Pitch-Rot(y,θ)

Roll-Rot(x,Φ)

xz

y

x

zy

x

zy

x

z

y


x

y

{E}

x

y

z

Final configuration


• How can the robot position be defined?– UUV Position & attitude

Er=(x, y, z, ϕ, θ, ψ)T

– Homogeneous Matrix


• Kinematics Model of an Underwater Robot– Remember

Earth-fixed

X

Z

Y

DOF123456

Position and

Euler angles

xyZϕΘψ

Forces andmoments

XYZKMN

Lin. andang. Vel. u (surge) v (sway) w (heave) p (roll) q (pitch) r (yaw)

η relative toinertial frame

υ,τ relative tofixed-body frame

Body-fixed

ro

Y0

Z0

X0

q

r

pu

v

w

{B}

{E}


• Kinematics Model of an Underwater Robot– Relationship between Eη and Gυ

• Linear Velocity

• Angular Velocity

.

x xz

y

x

zy

x

zy

x

z

y

{1}

{2}

{B}

{E}

Yaw-Rot(z,ψ)

Pitch-Rot(y,θ)

Roll-Rot(x,Φ)


• Kinematics Model of an Underwater Robot– Relationship between Eη and Gυ

.


• Kinematics Model of an Underwater Robot

Kinematics

Kinematics Simulation

KinematicsBvInverse

KinematicsBv .

Eη

Direct Kinematics Inverse Kinematics

Eη.

{E}

{B}

x

yz

xyz


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


• 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 Robot senses a door


The Robot Moves




ut=2 m, xt-1=5 m5 7 9 11

2

ut=4 m, xt-1=5 mut=6 m, xt-1=5 m

ut

xt-1

ut=2 m, xt-1=3 m3 5 7 9

2

ut=4 m, xt-1=3 mut=6 m, xt-1=3 m ut

xt-1

46

64

Robot belief of being at state xt-1

ut


State Transition probability


Robot belief of being at state xt-1

ut

State Transition probability

ut=2 m, xt-1=5 m

5 7

2

Prior Belief. Prediction of state xt.

Measurement probability

Robot belief of being at state xt


Documents

Probabilistic Robotics Course Presentation Outline 1. Introduction 2. The Bayes Filter 3. Non Parametric Filters 4. Gausian Filters 5. EKF Map Based Localization