Interval Analysis for Kidnapping Problem using Range Sensors

(1)

The Global Localization Context The Proposed Method The Simulator

Interval Analysis for Global Localization Problem using Range Sensors

R´ emy GUYONNEAU, S´ ebastien LAGRANGE, Laurent HARDOUIN and Philippe LUCIDARME.

Laboratoire d’Ing´ enierie des Syst` emes Automatis´ es (LISA), University of Angers,

France.

June 10, 2011

R. Guyonneau, S. Lagrange, L. Hardouin, P. Lucidarme Interval Analysis for Global Localization Problem using Range Sensors 1

(2)

Introduction

F Robot localization is an important issue of mobile robotics.

F Robotics challenge CAROTTE ¹

F Simultaneous Localization And Mapping (SLAM) and Global Localization problems.

F In this presentation a set membership approach will be considered to deal with the global localization problem.

1

CArtographie par ROboT d’un TErritoire (Robot Land Mapping) organized by the french ANR (National Research Agency).

(3)

Outline

1 The Global Localization Context

2 The Proposed Method The Static localization

The Global Localization Algorithm

3 The Simulator

(4)

The Robot The Environment The Objective

The Global Localization Context

1 The Global Localization Context

2 The Proposed Method The Static localization

The Global Localization Algorithm

3 The Simulator

(5)

The Robot

The considered robot

We consider a mobile wheeled robot with a LIDAR ^a sensor. Its pose is defined by p =



 x y θ



, with (x, y) its localization and θ

its orientation.

a

Light Detection And Ranging

The measurements

The sensor provides a set of n measurements:

D =







d ₀ = (d ₀

_x

, d ₀

_y

) .. . d n = (d n

x

, d n

y

)







(6)

The Robot

Robot ^Obstacle

Figure 1: The robot’s pose.

Robot ^Obstacle

Figure 2: A measurement d

i

.

(7)

The Environment

The map

The known environment E ∈ R ² is discretized with a resolution δ _x , δ y and this lead to a grid G composed of n × m cells (i, j ). At each cell (i , j ) is associated g _i _,j ∈ {0, 1}:

g _i _,j =

( 1 if there is an obstacle in the cell (i , j ), 0 else.

(8)

The Objective

Hypotheses

, → Bounded error context, , → Outliers can be

considered,

Figure 3: Measurements from the sensor.

(9)

The Objective

Figure 2: We have a grid map.

Hypotheses

, → Bounded error context, , → Outliers can be

considered,

Figure 3: Measurements from the sensor.

(10)

The Objective

Figure 2: We have a grid map.

Hypotheses

, → Bounded error context, , → Outliers can be

considered,

Figure 3: Measurements from the sensor.

(11)

The Objective

Figure 2: We have a grid map.

Hypotheses

, → Bounded error context, , → Outliers are considered,

Figure 4: Initial domain.

(12)

The Objective

(13)

The Objective

(14)

The Objective

(15)

The Objective

(16)

The Objective

The robot is localized

All the measurements fit with the map.

(17)

The Objective

The robot is localized Except for one outlier.

(18)

The Static localization The Global Localization Algorithm

The Proposed Method

1 The Global Localization Context

2 The Proposed Method The Static localization

The Global Localization Algorithm

3 The Simulator

(19)

The Static localization

The Context Let [p] =



 [x]

[y]

[θ]



 be an initial domain that encloses the robot’s

pose p =



 x y θ



 and D =





 d 0

.. . d n





 a set of n measurements.

(20)

The Static localization

The Robot-Measurement Distance

||d _i || ² = (x − w _i

_x

) ² + (y − w _i

_y

) ² .

Robot Obstacle

(21)

The Static localization

The Measurement-Measurement Distance

||d _i − d _j || ² = (w _i

_x

− w _j

_x

) ² + (w _i

_y

− w _j

_y

) ² .

Robot Obstacle

(22)

The Static localization

The Measurements Coordinates

The coordinates (w i

x

, w i

y

) of an obstacle in the map are defined by:

w _i

_x

w _i

_y

=

cos(θ) sin(θ)

−sin(θ) cos(θ)

d _i

_x

d _i

_y

+

x y

Robot Obstacle

(23)

The Static localization

(24)

The Static localization

(25)

The Static localization

(26)

The Static localization

The map constraint.

We define c _G the constraint which says that the

measurement has to be consistent with the map.

(27)

The Static localization

(28)

The Static localization

The map contractor.

We define C _G the contractor of the constraint c _G .

(29)

The Static localization

(30)

The Static localization

The map constraint.

We define c _G the constraint which says that the measurement has to be consistent with the map.

The map contractor.

We define C _G the contractor of the constraint c _G .

(31)

The Static localization

Considered Constraint Satisfaction Problem V = {x, y, θ,

d _i = (d _i

_x

, d _i

_y

) ^T , w _i

_x

, w _i

_y

with i = 1, · · · , n}, D = {

[x ] = [−∞, +∞], [y ] = [−∞, +∞], [θ] = [0, 2π], [w

i_x

] = [−∞, +∞], [w

i_y

] = [−∞, +∞],

[d

i_x

] =obtained from the sensor, [d

i_y

] =obtained from the sensor}.

C =



 



 



c w

_ix

: d _i

_x

cos(θ) + d _i

_y

sin(θ) + x c _w

_iy

: −d _i

_x

sin(θ) + d _i

_y

cos(θ) + y c _d

_i

: ||d _i || ² = (x − w _i

_x

) ² + (y − w _i

_y

) ²

c _d

_i,j

: ||d _i − d _j || ² = (w _i

_x

− w _j

_x

) ² + (w _i

_y

− w _j

_y

) ² c _G : to be consistent with the map (using C _G )

(32)

The Global Localization Algorithm

1 The Global Localization Context

2 The Proposed Method The Static localization

The Global Localization Algorithm

3 The Simulator

(33)

The Global Localization Algorithm

Assumption

We assume u = (u r , u _l ) the knowledge of the moving of the robot, with u _r the speed of the right wheel and u _l the speed of the left wheel.

(34)

The Global Localization Algorithm

Prediction Equation

f (p _k , u _k ) = p _k ₊₁ =



 x k+1

y _k+1 θ _k+1



 =







x _k + ^u

^kr

^+u ₂

^kl

cos(θ _k ) y _k + ^u

^kr

^+u ₂

^kl

sin(θ _k )

θ _k + ^u

^kr

^−u ₂

^kl





 .

Retro-propagation Equation

f ⁻¹ (p _k+1 , u k ) = p _k =







x k+1 − ^u

^kr

^+u ₂

^kl

cos(θ k+1 − ^u

^kr

^−u ₂

^kl

) y _k+1 − ^u

^kr

^+u ₂

^kl

sin(θ _k+1 − ^u

^kr

^−u ₂

^kl

)

θ _k+1 − ^u

^kr

^−u ₂

^kl





 .

(35)

The Global Localization Algorithm

Three Constraints

To be consistent with a data set and the map: The static localization.

Retro-propagation : p _k−1 = f ⁻¹ (p _k , u k−1 ), Prediction : p _k+1 = f (p _k , u k ),

with p _k−1 ∈ P k−1 , p _k ∈ P k , p _k+1 ∈ P k+1 , u k−1 ∈ [u k−1 ] and u _k ∈ [u _k ].

(36)

The Global Localization Algorithm

(37)

The Global Localization Algorithm

(38)

The Global Localization Algorithm

(39)

The Global Localization Algorithm

(40)

The Global Localization Algorithm

(41)

The Global Localization Algorithm

(42)

The Global Localization Algorithm

(43)

The Global Localization Algorithm

(44)

The Global Localization Algorithm

(45)

The Global Localization Algorithm

(46)

Demonstration

The Simulator

1 The Global Localization Context

2 The Proposed Method The Static localization

The Global Localization Algorithm

3 The Simulator

(47)

Demonstration

Simulation parameters

I A 1000 × 1000 cells occupancy grid map (10 × 10 meters).

I Each measurement has a bounded error of 10 centimetres and a max range d max = 3 meters.

(48)

Demonstration

(49)

Conclusion

F In this presentation we have seen that interval analysis could be used to solve the global localization problem.

F This method uses a discrete map so the time processing depends of the size of the grid.

F The simulation results are promising and this method can be efficient in a real context.

Interval Analysis for Kidnapping Problem using Range Sensors

Interval Analysis for Global Localization Problem using Range Sensors

R´ emy GUYONNEAU, S´ ebastien LAGRANGE, Laurent HARDOUIN and Philippe LUCIDARME.

Laboratoire d’Ing´ enierie des Syst` emes Automatis´ es (LISA), University of Angers,

France.

June 10, 2011

Introduction

F Robot localization is an important issue of mobile robotics.

F Robotics challenge CAROTTE 1

F Simultaneous Localization And Mapping (SLAM) and Global Localization problems.

F In this presentation a set membership approach will be considered to deal with the global localization problem.

CArtographie par ROboT d’un TErritoire (Robot Land Mapping) organized by the french ANR (National Research Agency).

Outline

1 The Global Localization Context

2 The Proposed Method The Static localization

The Global Localization Algorithm

3 The Simulator

The Global Localization Context

1 The Global Localization Context

2 The Proposed Method The Static localization

The Global Localization Algorithm

3 The Simulator

The Robot

The considered robot

We consider a mobile wheeled robot with a LIDAR a sensor. Its pose is defined by p =



 x y θ



, with (x, y) its localization and θ

its orientation.

Light Detection And Ranging

The measurements

The sensor provides a set of n measurements:

D =







d 0 = (d 0

, d 0

) .. . d n = (d n

, d n

)







The Robot

Figure 1: The robot’s pose.

Figure 2: A measurement d

.

The Environment

The map

The known environment E ∈ R 2 is discretized with a resolution δ x , δ y and this lead to a grid G composed of n × m cells (i, j ). At each cell (i , j ) is associated g i ,j ∈ {0, 1}:

g i ,j =

( 1 if there is an obstacle in the cell (i , j ), 0 else.

The Objective

Hypotheses

, → Bounded error context, , → Outliers can be

considered,

Figure 3: Measurements from the sensor.

The Objective

Figure 2: We have a grid map.

Hypotheses

, → Bounded error context, , → Outliers can be

considered,

Figure 3: Measurements from the sensor.

The Objective

Figure 2: We have a grid map.

Hypotheses

, → Bounded error context, , → Outliers can be

considered,

Figure 3: Measurements from the sensor.

The Objective

Figure 2: We have a grid map.

Hypotheses

, → Bounded error context, , → Outliers are considered,

Figure 4: Initial domain.

The Objective

The Objective

The Objective

The Objective

F Robotics challenge CAROTTE ¹

We consider a mobile wheeled robot with a LIDAR ^a sensor. Its pose is defined by p =

d ₀ = (d ₀

, d ₀

The known environment E ∈ R ² is discretized with a resolution δ _x , δ y and this lead to a grid G composed of n × m cells (i, j ). At each cell (i , j ) is associated g _i _,j ∈ {0, 1}:

g _i _,j =

||d _i || ² = (x − w _i

) ² + (y − w _i

) ² .

||d _i − d _j || ² = (w _i

− w _j

) ² + (w _i

− w _j

) ² .

w _i

w _i

d _i

d _i

We define c _G the constraint which says that the

We define C _G the contractor of the constraint c _G .