probabilistic robotics course presentation outline 1. introduction 2. the bayes filter 3. non...

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

Probabilistic Robotics

Where am I?

Localization

Where are theamphoras?

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 1376.

1 Introduction

Probabilistic Robotics

This is an old problem…

1 Introduction

Probabilistic Robotics

Map Robot PoseEnvironment

Localization Algorithms

Mapping Algorithms

SLAM: Simultaneous Localization And Mapping

1 Introduction

Probabilistic Robotics

> Localization Through SLAM:

UPDATE

{B}

Y

X

MEASUREMENT

External Sensors

PREDICTION

InternalSensors

Step: k+1

Step: k

DATAASSOCIATION

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

Xx

y

{E}

{G}

n

o {E} x

yz

y x

Z

Probabilistic Robotics

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

Probabilistic Robotics 6 DOF Dead Reckoning From Velocity

Position + Roll, Pitch, Yaw attitude

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

Homogeneous Matrix

xz

y xz

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

zy

x

zy

xz

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

xz

y

x

zy

• 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

zYaw-Rot(z,ψ)

Pitch-Rot(y,θ)

Roll-Rot(x,Φ)

xz

y

x

zy

x

zy

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

Er=(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

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}

Probabilistic Robotics 6 DOF Dead Reckoning From Velocity

• 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,Φ)

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

KinematicsBvInverse

KinematicsBv .

Eη

Direct Kinematics Inverse Kinematics

Eη.

{E}

{B}

x

yz

xyz

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

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

1.3 Localization Example

State Transition probability

Probabilistic Robotics

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

1.3 Localization Example