PATH PLANNING FOR MULTIPLE FEATURES BASED

LOCALIZATION

Francis Celeste, Frederic Dambreville

Dept. EORD FAS Team, DGA, 16 bis av. Prieur de la cte d’Or, 94114 Arcueil, France

Jean-Pierre Le Cadre

IRISA/CNRS, Campus de Beaulieu, 35042 Rennes, France

Keywords:

Path planning, Cram`er Rao Bound, map-based localization, dynamic programming.

Abstract:

In surveillance or exploration mission in a known environment, the localization of the dedicated sensor is of

main importance. In this paper, we discuss the path planning problem for the localization algorithm which

correlates range and bearing measurements and a map composed of several features. The sensor motion

is designed from an information measure derived from the Fisher Information Matrix. It is shown that a

closed form expression of the cost can be obtained. The optimal features location can be neatly geometrically

interpreted. An integral cost which includes the sensor perception limitation is then formulated. It is used in a

dynamic programming framework to tackle the path optimization problem.

1 INTRODUCTION

The path planning problem for map-based localiza-

tion consists in designing the best trajectory for a mo-

bile in a known environment, which guarantees the

highest performance of positioning during its execu-

tion. Data collected from sensors are “matched” to

a prior map to estimate the state (e.g., position and

heading). Depending on the sensor dynamic and the

observation models, different localization algorithms

can be used. When the system is linear or near linear

with Gaussian noises, Kalman-based approaches are

relevant (Thrun et al., 2005; S. Thrun and Dellaert,

2000). In this paper, we introduce a framework to

compute “optimal” path for a moving vehicle which

collects range and bearing data from 2D features. One

of the main challenges is to choose an appropriate

measure to be optimized. In random estimation, the

Fisher Information Matrix (FIM) can be used. We

considered a D-optimal design (Paris and Le Cadre,

2002). The ﬁrst interesting result of this work is the

derivation of a closed form expression for the FIM

determinant. It is shown that it depends on groups of

two or three features. Then, a geometric analysis of

the optimal features placement can be done. By ex-

ploiting this measure, we introduce an integral cost

functional for a path space, which is composed of el-

ementary moves with constant velocity and constant

heading. Moreover, the sensor ﬁeld of view limita-

tions are included to the cost computation. At last, we

formulate the problem as ﬁnding an optimal path on a

graph by means of dynamic programming. The paper

ends with one illustrative example.

2 PROBLEM FORMULATION

We consider a moving sensor evolving according to

the dynamic model

˙x

= v

cosϕ

˙y

= v

sinϕ

= ω

. (1)

where its state X

∆

= [x

,ϕ

] is composed of its

2-D position and its orientation. A feature map of

its environment is available for localization purpose.

In equation 2, we assume that the known control

∆

= [v

,ω

] ∈ U ⊂ R

. During its displacement, the

mobile gets sensor measurements from detected fea-

tures which are in the embedded map. Let us denote

∆

= { f

,..., f

} the set of m

features visible and

used in the localization process at time t. Each fea-

ture is deﬁned by its 2D position in a global frame

214

Celeste F., Dambreville F. and Le Cadre J. (2008).

PATH PLANNING FOR MULTIPLE FEATURES BASED LOCALIZATION.

In Proceedings of the Fifth International Conference on Informatics in Control, Automation and Robotics - ICSO, pages 214-219

DOI: 10.5220/0001498102140219

 SciTePress

∆

= (O,

−→

u ,

−→

v ):

↔





∈ D ⊂ R

. (2)

and the “sensor-feature” vector δp

(t)

∆



− x

, y

− y



∗

. The measurements vector is

the stacked vector Z



,...,z



where z

is the

range and bearing measurement for feature f

. So,

the observation model stands as follows :

= H

) + W

. (3)

where the 2×i

and 2×i+ 1

elements of H

)

are the components of the two dimensional vector

h(X

, f

) given by

= h(X

, f

) + w

. (4)

h(X

, f

)

∆

(

− x

)

+ (y

− y

)

atan

(

−y

−x

) − ϕ

(5)

The noise vector w

is modelled by an i.i.d. Gaus-

sian process with zero mean and covariance matrix

. Moreover, we suppose that Σ

= Σ,∀i and

Σ =



0 σ



. (6)

We also consider that w

and w

are independent for

l 6= j. So in light of (2), the likelihood function is

given by

p(Z

) ∝ exp

−

∑

l=1

− h(X

, f

. (7)

is one estimate based on the measurement Z

(e.g., the maximum likelihood estimate), the covari-

ance error e

= X

−

is lower bounded by the

Cramer Rao Bound (CRB) (Van Trees, 1968).

Cov(e

) ≻ F

−1

(t). (8)

The calculation of the FIM F(t) is given in our case

by,

F =

∑

i=1



∂h(X

, f

)

∂X



∗

−1



∂h(X

, f

)

∂X



. (9)

The elementary gradient vector can be derived

straightforwardly

∂h(X

, f

)

∂X



−

−1



. (10)

where α

(t)

∆

= ∠

−→

u δp

(t), ρ

∆

= ||δp

(t)||, c

∆

= cosα

and s

∆

= sinα

. Let us also introduce the following

notations :

• ~c

∆

= [c

···c

]

∗

,~s

∆

= [s

···s

]

∗

• ~c

∆

= [

···

]

∗

,~s

∆

= [

···

]

∗

• 1

∆

= [1···1]

∗

, 0

∆

= [0···0]

∗

Without loss of generality, we set σ

= σ

= 1 then

we can rewrite

F(t) = G(t)G(t)

∗

. (11)

with

G(t) =

(t)

z}|{

(t)

z}|{





−~c





(12)

G(t) is a 3 × 2m

matrix with columns G

are part of

the subset G

(t) or G

(t) :

(t) =



,1 ≤ i

≤ m





∗



(t) =

,1 ≤ i

≤ m



−



∗

In this paper, we are dealing with the optimization

of the sequence of displacement which provides the

“best” estimate of the state. This can be achieved us-

ing an appropriate measure of information gain. We

adopt here a D-optimal design considering the deter-

minant of the FIM

. In the next section, we show

that this measure is a function implying the esti-

mated bearings angles (α

(t))

i=1

and relative ranges

(ρ

(t))

i=1

3 DERIVATION OF det(F)

Let us deﬁne L (t) as the determinant of the FIM at

time t in position X

. From (11), we have

L (t) = det(G(t)G(t)

∗

). (13)

Using the Binet-Cauchy formula

, we can notice that

L (t) =

∑

1≤i< j<k≤2m



det(G

)



. (14)

hence to compute L (t), we have to enumerate the

different cases in accordance with the column vec-

tors (G

) are in G

or G

. In the following,

we denote d

ijk

∆

= det(G

). If all columns are

in G

, d

ijk

is trivially equal to zero. Using determi-

nant computation properties and relations betweeen

trigonometric functions, we get

* is the transpose operator

other matrix operator can be used, such as the trace

det(AB) =

∑

det(A

)det(B

), S = {1,·· · , n}, if A ∈

(m,n) et B ∈ M

(n,m), A

is the m× n matrix whose

columns are those of A with in S

PATH PLANNING FOR MULTIPLE FEATURES BASED LOCALIZATION

215

a) G

∈ G

and G

∈ G

ijk

= sin(α

− α

b) G

∈ G

and G

, G

∈ G

ijk

cos(α

− α

)

−

cos(α

− α

)

c) G

∈ G

, G

and G

∈ G

ijk

sin(α

− α

)

sin



− α



sin



− α



In conclusion, we notice that L (t) is the sum of three

terms L

(t), L

(t)andL

(t) which characterize inter-

actions between pairs and triplets of visible features.

L (t) = a

(t) + a

(t). (15)

with L

(t) =

∑

i=1

∑

j>i

( f

, f

), L

(t) =

∑

i=1

∑

j=1

∑

k> j

( f

, f

) and L

(t) =

∑

i=1

∑

j>i

∑

k> j

( f

, f

) where (g

)

l∈{1,2,3}

are respectively given by the square of d

ijk

in the

above cases. Coefﬁcients (a

)

1≤l≤3

depend on σ

and

4 THE OPTIMAL PLACEMENT

OF THE FEATURES

We now study the location of the features which pro-

vides the best performance of estimation around a

given mean state

X. The analysis takes into account

the sensor ﬁeld of view and only consider L

(t) (pairs

interaction). Such an approximation is valid when

≪ σ

. Let ( f

)

1≤i≤n

be visible from state

X. We

introduce P = (¯x; ¯y), (~v

)

1≤i≤n

, D

, ~v

−

and ~v

(see

ﬁgure 1). D

is the angular aperture of the sensor ﬁeld

of view. An analogy can be made with the reasoning

v−

2α

Figure 1: Sensor features spatial conﬁguration.

in (Gu et al., 2006) for multiple UAVs cooperation

for sensing. The derivation made here is nevertheless

simpler and more geometrically intuitive.

Proposition 1

. Maximizing L

(t) is equivalent to

ﬁnd the conﬁguration (~v

∗

,...,~v

∗

) which minimizes

||~v

|| = ||

∑

i=1

Indeed, using classic trigonometric properties

can show that L



1− ||

∑

i=1



4.1 Optimal Placement for D

In this context, the value of the angle made by vectors

and ~v

is strictly smaller than π. So ||~v

|| > 0. Let

∈ {1,··· ,n} and θ

= ∠ ~v

−

. We also denote ~v

∆

∑

j6=i

and θ

= ∠ ~v

−

||~v

= ||~v

+ ~v

= 1+ ||~v

+ 2||~v

||cos



− θ



As D

, ~v

is also between ~v

−

and ~v

. So, for

a given placement of vectors {~v

}

i6=i

, ||~v

|| is min-

imized for θ

∗

which makes g(θ

) = cos



− θ



minimum.

Proposition 2. In the optimal conﬁguration, each

vector ~v

is on the frontier of the visibility cone.

Proof. 0 ≤ θ

,θ

≤ 2D

⇒ θ

− 2D

≤ θ

− θ

≤

. Moreover, θ

− 2D

> −π et θ

< π. We can

easily deduce that

∗

(

if |θ

− 2D

| > θ

0 if |θ

− 2D

| < θ

which proves that either ~v

= ~v

−

or ~v

= ~v

. Let us

denote n

−

and n

the number of vectors ~v

respec-

tively equal to ~v

−

and ~v

−

+ n

= n). n

−

must

verify the relation

||~v

= 2(1− a)n

−

− 2(1− a)nn

−

+ n

∆

= f(n

−

with a = cos(2D

) (a < 1). f is minimal for n

−

• if n is even, n

−

= n

and which provides

sin

) .

• else we can set n

−

n−1

and n

n+1

, then

− 1

sin

) .

sin

a =

(1 − cos2a) and cos(a − b) = cosacosb +

sinasinb

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

216

4.2 Optimal Placement for D

In this case, we have to make a different reasoning ac-

cording to the parity of n. When n is even, the optimal

solution is obvious as we can place the features so that

0. Indeed, it is enough to choose {~v

,··· ,~v

}

pairwise such that their difference angle is equal to π

(i.e. orthogonal assignment of features). We can no-

tice that, there are plenty of such conﬁgurations and

the cost is L

. Otherwise, if n is odd, it is more

difﬁcult to ﬁnd a placement which gives ~v

0. Nev-

ertheless, we can search among a particular class of

conﬁgurations with ~v

= −~v

. Assuming i

= n, one

way to obtain ~v

collinear and opposite to ~v

, is to

choose {~v

,··· , ~v

n−1

} where

∃ϕ ∈

,π



∠~v

= ϕ, ∀i ∈ {1, · ·· ,

n−1

∠~v

= ϕ, ∀ j

= i+

n−1

Given ∠ ~v

−

= θ

, ∀i ∈ {1, · ·· ,

n−1

} and supposing

−

=~u, then

= cos(ϕ+ θ

)~u+ sin(ϕ+ θ

)~v,

= cos(θ

)~u+ sin(θ

)~v, ∀i,

= cos(2ϕ+ θ

)~u+ sin(2ϕ+ θ

)~v, ∀ j

and ∀i ∈ {1,··· ,

n−1

}

+ ~v

= cos(θ

) + cos(2ϕ+ θ

)~u

+sin(θ

) + sin(2ϕ+ θ

)~v. (16)

Using trigonometric properties, we get that:

+ ~v

= 2cos(ϕ)(cos(ϕ+ θ

)~u+ sin(ϕ+ θ

)~v)

= 2cos(ϕ)~v

To make ~v

0 , we must force

n−1

∑

i=1

+ ~v

which is equivalent to the following condition on ϕ.

l(ϕ)

∆

= 1+ (n− 1)cos(ϕ) = 0, ϕ ∈

,π

. (17)

As the ﬁeld of view is limited, we have to satisfy

ϕ ≤ D

. Therefore, if such an angle exists, the cost

value is again L

. In particular, if D

2π

we can always ﬁnd an optimal placement. Indeed,

it is sufﬁcient to choose n − 3 vectors as in the even

case (orthogonal assignment) and to use the last three

with ϕ =

2π

. When exists ϕ solution of (17) with

< ϕ <

2π

, it seems difﬁcult to ﬁnd a conﬁguration

which allows to attain the maximum cost. But, we

propose a suboptimal solution which minimizes l(ϕ).

l is decreasing on





(

∂l

∂ϕ

∝ −sin(ϕ) < 0) so its

maximum is given for ϕ = D

. This leads to the cost

value



− (1+ (n− 1)cos(D

))



In this section, we made a geometric analysis to de-

termine the optimal placement of the features to max-

imize the cost L

. Making the same kind of reasoning

for the complete cost L (t) is much more challeng-

ing. After this static analysis, we deals with the path

planning problem in the next section. For the sake of

brevity, we only detail the approach for L

(t) but it

can be generalized to L

(t) and L

(t).

5 PATH PLANNING

We consider the evolution of the sensor between

] with 0 < t

≤ T∗ from position q

∈ D to po-

sition q

∈ D . We look for paths (X

)

t∈

[

]

which

maximizes the cost

Ψ([t

]) =

(t)dt. (18)

The problem can be formalize in the optimal control

framework with two boundaries constraints. Unfortu-

nately, due to the cost expression and the sensor ﬁeld

of view (FOV) limitations, no analytic formulation of

the optimal path can be derived. An approximated

approach based on the discretization of the state and

control space seems more tractable.

5.1 Path Description

As in (Celeste et al., 2007), We formalize here

the problem as a discrete path planning. A regu-

lar grid is considered and one path is a sequence

of elementary displacements with constant heading



ϕ ∈ { ϕ

i∗π

, i ∈ {−3,...,4}}



and constant veloc-

ity v (a leg). For a path τ with n

legs, the cost is as

follows:

Ψ([t

]) =

−1

∑

i=0

i+1

(t)dt. (19)

= q

and X

−1

= q

are supposed to be on the

grid. Some constraints on the maneuvers can be im-

posed to avoid chaotic behavior (e.g. bang-bang ef-

fect)(Paris and Le Cadre, 2002). To solve the planning

task we need to compute the cost associated with each

leg. First of all, it is necessary to determine the part of

the leg where each feature is visible due to the sensor

FOV.

PATH PLANNING FOR MULTIPLE FEATURES BASED LOCALIZATION

217

5.2 Cost for One Leg

For a FOV model with an aperture 2∆ and a maximum

range detection R

, the area Z visible from the leg e is

composed of three regions Z

, Z

and Z

(see Figure

3). A pair of features ( f

, f

) ∈ Z

are visible from

−

) and P

). These limits can be de-

rived using a simple geometric reasoning. Moreover,

Figure 2: The visible region for one leg.

we have a relation between an elementary displace-

ment and the associated duration (dt ∝ dx if ϕ 6=

[π],

dt ∝ dy else). and the leg can be reparametrized as

follows:

• y(x) = β+ γx, ∀x ∈ [x

] if ϕ 6=

[π] (non verti-

cal motion),

• x = x

, y

≤ y ≤ y

else (vertical motion),

The total cost for a leg e can then be computed using

relevant change of variable.

For non vertical displacement, the cost due to

a pair of features ( f

, f

) is the integral of a rational

function:

(x) =



(x− x

)(y(x) − y

) − (x− x

)(y(x) − y

)



(x)p

(x)

where p

(x) = (x− x

)

+ (y(x)− y

)

∆

= a

+ b

, l ∈ { j, i} is the respective square range of f

, f

to the sensor. Therefore, these polynomials are irre-

ducible whatever the sensor position in D \



, f



We can rewrite

(x) =

x+ B

)

(x)p

(x)

. (20)

So, we have to compute:

(e) ∝

−

(x)dx. (21)

which can be done with a relevant partial expansion

of the rational function. Nevertheless, we have to pay

attention to the position of the leg relativelyto the fea-

tures.

case (1) e is on the perpendicular bisector of [ f

then p

(x) = p

(x),∀x and

x+ B

)

(x)p

(x)

x+ s

(x)

x+ s

(x)

. (22)

case (2) e is not on the perpendicular bisector of

[ f

], then

x+ B

)

(x)p

(x)

x+ s

(x)

x+ s

(x)

. (23)

Identiﬁcation of the numerators yields in both

cases to a linear system to deduce χ = [r

]

∗

(c)

χ = B

, for cases c = 1, 2 (24)

(1)







0 0 0

0 a

1 b

0 0 c







, B













(25)

and

(2)







0 0

0 0 c







(26)

For vertical displacements, it is more appropriate to

consider integration with the variable y. The same

reasoning leads to the integration of a rational func-

tion to get the cost expression

(e) ∝

−

(y)dy. (27)

5.2.1 Closed Form Expression for the Cost

Whatever the leg orientation, we have to deals with

the computation of integrals of the form (n ∈ {1,2},

l ∈ {i, j}):

(n)

(l,u, v, x

−

,x+) =

−

ux+ v

(ax

+ bx+ c)

dx (28)

Using speciﬁc changes of variable and classic prim-

itives, the closed form expression for the cost (21),

(27) can be derived. For instance,

(1)

(l,u, v, x

−

,x+) = ν

(1)



−



(1)



tan

−1

)) − tan

−1

−

))



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

218

where q

−b

, ν

(1)

and

(1)

2va

−ub

The expressions of the costs are ﬁnally

(e) = H

(1)

(i,r

−

) + H

(n)

( j, r

−

)

where n ∈ {1,2} depends on the leg orientation ac-

cording to [ f

, f

]. Given the contribution of each vis-

ible pair of features, the complete cost of the leg is

given by c(e) =

∑

i, j

(e) Therefore, the cost associ-

ated to a path τ = {e

,··· ,e

} of length n = n

− 1

is c(τ) =

∑

i=1

c(e

). The optimization can then be

solved via dynamic programming.

6 EXPERIMENT

In this experiment, we consider an embedded map

composed of ten features organised on the border of

D = [0;200;0;200]. The sensor FOV is character-

ized by a maximum range detection R

max

= 70m and a

half aperture angle D

= 120 deg.. Moreover, the au-

thorized difference angle between two following time

steps must be bounded by π/4 and the path length

smaller than l

max

= 98 legs from q

= (20;20) to

= (170;20). The grid resolutions are δx= δy = 10.

The algorithm seems to behave well. The sensor

0 20 40 60 80 100 120 140 160 180 200

100

120

140

160

180

200

path planning

Figure 3: Optimal path, features(green), q

and q

(blue).

moves in order to be as soon as possible on the per-

pendicular bisector of pairs of features and to increase

the number of visible pairs. The proposed path allows

to provide better triangulation conditions which im-

proves the estimation process. Moreover some inter-

esting behaviour like cycles can also be observed.

7 CONCLUSIONS AND

PERSPECTIVES

In this paper, we introduced a path planning algorithm

for map based localization. First of all, we derived an

information gain as the determinant of the Fisher In-

formation Matrix adapted to multiple features. A geo-

metric interpretation of this measure was made. Then,

to determine the optimal path, we considered the in-

tegral cost of this function. It is important to notice

that the cost computation take into account the sensor

ﬁeld of view model. Finally, we applied the approach

on a scenario and illustrate the behaviour of the algo-

rithm. We detailed the approach for only the ﬁrst part

of the total cost, but it can be generalized to the oth-

ers. Now, we plan to take into account noisy feature

positions which will yieldsto a path planning problem

with uncertain cost. Then, the next challenge is to ﬁnd

optimal paths which tackle also those uncertainties on

the given map.

REFERENCES

Celeste, F., Dambreville, F., and Le Cadre, J.-P. (2007). Op-

timal strategies based on the cross entropy method. In

Fusion 2007, Quebec (Canada).

Gu, G., Chandler, P., Schumacher, C., Sparks, A., and

Pachter, M. (2006). Optimal cooperating sensing us-

ing a team of uavs. IEEE Trans. on Aerospace and

Electronic Systems, 42(4).

Paris, S. and Le Cadre, J.-P. (2002). Trajectory planning for

terrain-aided navigation. In Fusion 2002.

S. Thrun, D. Fox, W. B. and Dellaert, F. (2000). Robust

monte carlo localization for mobile robots. Artiﬁcial

Intelligence, 28(1-2):99–141.

Thrun, S., Burgard, W., and Fox, D. (2005). Probabilistic

Robotics. MIT press.

Van Trees, H. (1968). Detection, Estimation and Modula-

tion Theory. New York Wiley.

PATH PLANNING FOR MULTIPLE FEATURES BASED LOCALIZATION

219