A SET APPROACH TO THE SIMULTANEOUS LOCALIZATION

AND MAP BUILDING

Application to Underwater Robots

Luc Jaulin

, Fr

eric Dabe

, Alain Bertholom

and Michel Legris

E3I2, ENSIETA, 2 rue Franois Verny, 29200 Brest

GESMA, Brest

Keywords:

Bounded-error, constraint propagation, interval analysis, SLAM, state estimation, submarine robots, robotics.

Abstract:

This paper proposes a set approach for the simultaneous localization and mapping (SLAM) in a submarine

context. It shows that this problem can be cast into a constraint satisfaction problem which can be solve

efﬁciently using interval analysis and propagation algorithms. The efﬁciency of the resulting propagation

method is illustrated on the localization of submarine robot, named Redermor. The experiments have been

collected by the GESMA (Groupe d’Etude Sous-Marine de l’Atlantique) in the Douarnenez Bay, in Brittany.

1 INTRODUCTION

This paper deals with the simultaneous localization

and map building problem (SLAM) in a submarine

context (see (Leonard and Durrant-Whyte, 1992) for

the general SLAM problem). A set membership ap-

proach for SLAM (see e.g., (Marcoet al., 2000)) will

be considered and it will be shown that this approach

leads us to a constraints satisfaction problem (CSP)

(see e.g., (Jaulin et al., 2001) for notions related CSP

and applications) which can be solved efﬁciently us-

ing interval constraints propagation. The efﬁciency of

the approach is illustrated on an experiment where an

actual underwater vehicle is involved. In this prob-

lem, we try to ﬁnd an envelope for the trajectory of

the robot and to compute sets which contains some

detected sea marks (such as mines).

Set-membership methods have often been consid-

ered for the localization of robots (see, e.g., (Meizel

et al., 1996), (Halbwachs and Meizel, 1996), in the

case where the problem is linear). In situations where

strong nonlinearities are involved, interval analysis

has been shown to be particularly usefull (see e.g.

(Meizel et al., 2002), where one of the ﬁrst local-

ization of an actual robot has been solved with in-

terval methods). Interval analysis has been shown

to be efﬁcient in several SLAM applications (see

(Drocourt et al., 2005) and (Porta, 2005) where in-

terval analysis has already been used in the context

of SLAM for wheeled robots). But the approach

is here made more efﬁcient by the addition of con-

straint propagation techniques. Note that there ex-

ist many other robotics applications where interval

constraint propagation methods have been success-

ful (see, e.g., (Baguenard, 2005) for the calibration

of robots, (Raissi et al., 2004) for state estimation,

(Gning and Bonnifait, 2006) for dynamic localization

of robots, (Lydoire and Poignet, 2003), (Vinas et al.,

2006) for control of robots, (Delanoue et al., 2006)

for topology analysis of conﬁguration spaces, ...).

2 ROBOT

The robot to be considered in our application is an au-

tonomous underwater vehicle (AUV), named Reder-

mor (see Figure 1). This robot, developed by GESMA

(Groupe d’Etude Sous-Marine de l’Atlantique), has a

length of 6 m, a diameter of 1 m and a weight of 3800

Kg. It has powerful propulsion and control system

able to provide hovering capabilities. The main pur-

pose of the Redermor is to evaluate improved naviga-

tion by the use of sonar information. It is equipped

with a KLEIN 5400 side scan sonar which makes it

possible to localize objects such as rocks or mines.

It also encloses other sophisticated sensors such as a

Lock-Doppler to estimate its speed and a gyrocom-

Jaulin L., Dabe F., Bertholom A. and Legris M. (2007).

A SET APPROACH TO THE SIMULTANEOUS LOCALIZATION AND MAP BUILDING - Application to Underwater Robots.

In Proceedings of the Fourth International Conference on Informatics in Control, Automation and Robotics, pages 65-69

DOI: 10.5220/0001639000650069

 SciTePress

Figure 1: The Redermor inside the water and the boat from which it has been dropped.

pass to get its Euler angles.

3 METHOD

In the graphSLAM approach (Thrun and Montemerlo,

2005), a criterion is built by taking all constraints into

account. Then, a local minimization method, such

as conjugate gradient, is used to ﬁnd a good solution

of the SLAM problem. Here, we adopt a similar ap-

proach, but instead of building a criterion, we cast the

SLAM problem into a huge constraints satisfaction

problem (CSP). For our problem, these constraints are

given below.











t ∈ {6000.0, 6000.1,6000.2,.. . , 11999.4}, i ∈ {0,1,.. . , 11},

(t)

= 111120.

0 1

cos



(t)∗

180



(t)− `

R(t) =







cosψ

−sinψ

sinψ

cosψ

0 0 1













cosθ

0 sinθ

0 1 0

−sinθ

0 cos θ













1 0 0

0 cos ϕ

−sinϕ

0 sinϕ

cosϕ







p(t) = (p

(t), p

(t)), p(t + 0.1) = p(t) + 0.1 ∗ R(t).v

(t),

||m(σ(i)) − p(τ(i))|| = r(i),

(τ(i))(m(σ(i))− p(τ(i))) ∈ [0, 0] × [0, ∞] × [0, ∞],

(σ(i)) − p

(τ(i)) − a(τ(i)) ∈ [−0.5, 0.5]

In these constraints, p = (p

, p

) denotes cen-

ter of the robot, (ψ, θ, ϕ) denote the Euler angles of

the robot, σ (i) is the number of the ith detected ob-

ject, τ(i) is the time corresponding the ith detection

and m( j) is the location of the jth object. From this

CSP, a constraint propagation procedure (see, e.g.,

(Jaulin et al., 2001)) can thus be used to contract all

domains for the variables without loosing a single so-

lution.

4 RESULTS

A constraints propagation procedure has been used to

contract all domains of our CSP. The results obtained

are represented on Figure 2. Subﬁgure (a) represents a

punctual estimation of the trajectory of the robot. This

estimation has been obtained by integrating the state

equations from the initial point (represented on lower

part). We have also represented the 6 objects that have

been dropped at the bottom of the ocean during the ex-

periments. Subﬁgure (b) represents an envelope of the

trajectory obtained using an interval integration, from

a small initial box, obtained by the GPS at the begin-

ning of the mission. In Subﬁgure (c) a ﬁnal GPS point

has also been considered and a forward-backward

propagation has been performed up to equilibrium. In

Figure (d) the constraints involving the object have

been considered for the propagation. The envelope is

now thinner and enveloping boxes containing the ob-

jects have also been obtained (see Subﬁgure (e)). We

have checked that the unknown actual positions for

the objects (that have been measured independently

during the experiments) all belong to the associated

ICINCO 2007 - International Conference on Informatics in Control, Automation and Robotics

box, painted black. In Subﬁgure (f), a zooming per-

spective of the trajectory and the enveloping boxes

for the detected objects have been represented. The

computing time to get all these envelopes in less than

one minute with a Pentium III. About ten forward-

backward interval propagations have been performed

to get the steady box of the CSP.

In the case where the position of the marks is

approximately known, the SLAM problem translates

into a state estimation problem. The envelope for the

trajectory becomes very thin and a short computation

time is needed. The capabilities of interval propaga-

tion methods for state estimation in a bounded error

context have already been demonstrated in several ap-

plications (see e.g., (Gning, 2006), (Bouron, 2002),

(Baguenard, 2005), (Gning and Bonnifait, 2006),

(Jaulin et al., 2001)).

Figure 3 contains the reconstructed waterfall

(above) and one zoom (below). Each line corresponds

to one of the six seamarks (i = 0, .. . , 5) that have been

detected. The gray areas contains the set of all fea-

sible pairs (t,

p −m

), associated to the migration

hyperbola. The twelve small black disks correspond

to the detected marks. From each disk, we can get

the time t at which the mark has been detected (x-

axis), the number of the mark (corresponding to the

line), and the distance r

between the robot and the

mark ( y-axis). Black areas correspond to all feasible

(t,r

). Some of these areas are tiny and are covered

by a black disk. Some are larger and do not contain

any black disk. In such a case, an existing mark may

have been missed by the operator during the scrolling

of the waterfall. As a consequence, with the help of

Figure 3, the operator could scan again the waterfall

and ﬁnd undetected marks much more efﬁciently. If

the operator (which could be a human or a program) is

able to detect at least one more mark, then, the prop-

agation algorithm could be thrown once more to get a

thinner envelope for the trajectory, thinner black areas

in the reconstructed waterfall and thus a higher prob-

ability to detect new marks on the waterfall, . . . The

operator can thus be seen as a contractor ((Jaulin et

al., 2001)) inside a constraint propagation process.

5 CONCLUSION

In this paper, we have shown that interval con-

straints propagation could be applied to solve efﬁ-

ciently SLAM problems. The approach has been

demonstrated on an experiment made with an actual

underwater robot (the Redermor). The experiment

lasted two hours and involved thousands of data. If

all assumptions on the bounds of the sensors, on the

Figure 2: Results obtained by the interval constraint propa-

gation.

ﬂat bottom, on the model of the robot, . . . are satis-

ﬁed, then their exists always at least one solution of

our problem: that corresponding to the actual trajec-

tory of the robot.

When outliers occur during the experiment, our

approach is not reliable anymore and one should

take care about any false interpretation of the results.

Consider now three different situation that should be

known by any user of our approach for SLAM.

Situation 1 . The solution set is empty and an

empty set is returned by the propagation procedure.

Our approach detects that their exists at least one out-

lier but it is not able to return any estimation of the

trajectory and the positions of the marks. It is also

not able to detect which sensor is responsible for the

failure.

Situation 2. The solution set is empty but

nonempty thin intervals for the variables are re-

turned by the propagation. Our approach is not ef-

ﬁcient enough to detect that outliers exist and we can

wrongly interpret that an accurate and guaranteed es-

timation of the trajectory of the robot has been done.

Other more efﬁcient algorithms could be able to prove

that no solution exists which would lead us to the sit-

uation 1.

A SET APPROACH TO THE SIMULTANEOUS LOCALIZATION AND MAP BUILDING - Application to Underwater

Robots

Figure 3: The reconstructed waterfalls can help to ﬁnd undetected marks.

Situation 3. The solution set is not empty but

it does not contain the actual trajectory of the robot.

No method could be able to prove that outliers occur.

Again, our approach could lead us to the false conclu-

sion that a guaranteed estimation of the trajectory of

the robot has been done, whereas, the robot might be

somewhere else.

Now, for our experiment made on the Redermor,

it is clear that outliers might be present. We have ob-

served that when we corrupt some data voluntarily (to

create outliers), the propagation method usually re-

turns rapidly that no solution exists for our set of con-

straints. For our experiment, with the data collected,

we did not obtain an empty set. The only thing that we

can conclude is that if no outlier exist (which cannot

be guaranteed), then the provided envelope contains

the actual trajectory for the robot.

REFERENCES

X. Baguenard. Propagation de contraintes sur les inter-

valles. Application

a l’

etalonnage des robots. PhD dis-

sertation, Universit

e d’Angers, Angers, France 2005.

Available at: www.istia.univ-angers.fr/˜baguenar/.

P. Bouron. M

ethodes ensemblistes pour le diagnos-

tic, l’estimation d’

etat et la fusion de donn

ees tem-

porelles. PhD dissertation, Universit

e de Compi

egne,

Compi

egne, France, (Juillet 2002).

N. Delanoue, L. Jaulin and B. Cottenceau. Using interval

arithmetic to prove that a set is path-connected. The-

oretical Computer Science, Special issue: Real Num-

bers and Computers 351(1), 119–128, 2006.

C. Drocourt, L. Delahoche, B. M. E. Brassart and

A. Clerentin. Incremental construction of the robot’s

environmental map using interval analysis. Global

Optimization and Constraint Satisfaction: Second In-

ternational Workshop, COCOS 2003 3478, 127–141

2005.

A. Gning. Localisation garantie d’automobiles. Contribu-

tion aux techniques de satisfaction de contraintes sur

les intervalles. PhD dissertation, Universit

e de Tech-

nologie de Compi

egne, Compi

egne, France 2006.

A. Gning and P. Bonnifait. Constraints propagation tech-

niques on intervals for a guaranteed localization using

redundant data. Automatica 42(7), 1167–1175 (2006).

E. Halbwachs and D. Meizel. Bounded-error estimation for

mobile vehicule localization. CESA’96 IMACS Mul-

ticonference (Symposium on Modelling, Analysis and

Simulation) pp. 1005–1010 (1996).

L. Jaulin, M. Kieffer, O. Didrit, E. Walter. Applied Inter-

val Analysis, with Examples in Parameter and State

Estimation, Robust Control and Robotics. Springer-

Verlag, London (2001).

J. J. Leonard and H. F. Durrant-Whyte. Dynamic map build-

ing for an autonomous mobile robot. International

Journal of Robotics Research 11(4) (1992).

F. Lydoire and P. Poignet. Nonlinear predictive control us-

ing constraint satisfaction. In 2nd International Work-

shop on Global Constrained Optimization and Con-

straint Satisfaction (COCOS), pp. 179–188 (2003).

M. D. Marco, A. Garulli, S. Lacroix and A. Vicino. A

set theoretic approach to the simultaneous localization

and map building problem. In Proceedings of the 39th

IEEE Conference on Decision and Control, vol. 1, pp.

833–838, Sydney, Australia (2000).

D. Meizel, O. L

eque, L. Jaulin and E. Walter. Initial local-

ization by set inversion. IEEE transactions on robotics

and Automation 18(6), 966–971 (2002).

D. Meizel, A. Preciado-Ruiz and E. Halbwachs. Esti-

mation of mobile robot localization: geometric ap-

proaches. In M. Milanese, J. Norton, H. Piet-Lahanier

ICINCO 2007 - International Conference on Informatics in Control, Automation and Robotics

and E. Walter, editors, Bounding Approaches to Sys-

tem Identiﬁcation, pp. 463–489. Plenum Press, New

York, NY (1996).

J. Porta. Cuikslam: A kinematics-based approach to slam.

In Proceedings of the 2005 IEEE International Con-

ference on Robotics and Automation, pp. 2436–2442,

Barcelona (Spain) (2005).

T. Raissi, N. Ramdani and Y. Candau. Set membership

state and parameter estimation for systems described

by nonlinear differential equations. Automatica 40,

1771–1777 (2004).

S. Thrun and M. Montemerlo. The GraphSLAM algo-

rithm with applications to large-scale mapping of ur-

ban structures. International Journal on Robotics Re-

search 25(5/6), 403–430 (2005).

P. H. Vinas, M. A. Sainz, J. Vehi and L. Jaulin. Quantiﬁed

set inversion algorithm with applications to control.

Reliable computing 11(5), 369–382 (2006).

A SET APPROACH TO THE SIMULTANEOUS LOCALIZATION AND MAP BUILDING - Application to Underwater

Robots