MINIMUM COST PATH SEARCH IN AN

UNCERTAIN-CONFIGURATION SPACE

Eric Pierre and Romain Pepy

Institut d’

Electronique Fondamentale

Universit

e Paris XI

Orsay, France

Keywords:

Path planning, uncertainty, A* algorithm.

Abstract:

The object of this paper is to propose a minimum cost path search algorithm in an uncertain-conﬁguration

space and to give its proofs of optimality and completeness. In order to achieve this goal, we focus on one

of the simpler and efﬁcient path search algorithm in the conﬁguration space : the A* algorithm. We then

add uncertainties and deal with them as if they were simple dof (degree of freedom). Next, we introduce

towers of uncertainties in order to improve the efﬁciency of the search. Finally we prove the optimality and

completeness of the resulting algorithm.

1 INTRODUCTION

Today’s researches on mobile robots focalize on a

speciﬁc aspect: the autonomous navigation of a robot.

Navigating a robot generaly involves the use of a path

planner, which determines a path in a graph built from

the environment. However, where path planner tak-

ing into account geometric models of the environment

work well in simulation, the implementation of such

algorithms on real robots leads in most case to failure

to follow the path planned (Latombe, 1991). Thus,

searchers have decided to introduce the concept of

uncertainty, in order to generate a path that would

work even with uncertain data. This ﬁeld of research

has leaded to various works (Fraichard and Mermond,

1998; Lazanas and Latombe, 1995; Pepy and Lam-

bert, 2006).

The present paper is the following of the work in

(Lambert and Gruyer, 2003), which proposes a path

planner based on the A* algorithm ((Hart et al., 1968;

Nilsson, 1988; Russel and Norvig, 1995)) that takes

uncertainties into account. Using a simulator of exte-

roceptive and proprioceptive sensors and an Extended

Kalman Filter to process the sensors information, the

algorithm ﬁnds a safe path for a mobile robot in a

known-but-imperfect indoor environment despite the

uncertainties associated with the lack of precision of

the sensors. Unfortunately, the authors do not prove

the optimality and completeness of the algorithm they

propose.

The goal of the present paper is ﬁrstly to generalize

the algorithm given in (Lambert and Gruyer, 2003) to

any problem based on planning a path in a graph with

taking into account uncertainties on the graph’s pa-

rameters. Secondly, it is to prove the optimality and

completeness of this generalized algorithm.

In section 2, we build the mathematical structure

needed to explain the A* algorithm and to prepare the

proofs. Using the mathematical bases proposed in the

previous section, section 3 presents the A* while tak-

ing the uncertainties into account, which we will call

A* in an Uncertain Conﬁguration Space (AUCS*).

Finally, section 4 proposes a second and optimized

version of this algorithm (called Safe A* with Towers

of Uncertainties, or SATU*) using a new concept: the

towers of uncertainties (Lambert and Gruyer, 2003).

2 THE A* ALGORITHM

2.1 Overview

The famous A* algorithm (algorithm 1) is one of

the simpler and efﬁcient path planner in a graph.

This algorithm has ﬁrst been proposed in (Hart et al.,

103

Pierre E. and Pepy R. (2007).

MINIMUM COST PATH SEARCH IN AN UNCERTAIN-CONFIGURATION SPACE.

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

DOI: 10.5220/0001645301030109

 SciTePress

1968), and is based on the Dijkstra algorithm (Di-

jsktra, 1959) on which a heuristic function has been

added.

The algorithm searches the graph in order to ﬁnd a

path that goes from a start node to a goal node. When

we deal with a path planner, it is important to know its

behaviour, especially about two particular properties :

• Optimality: When the algorithm chooses a path to

go from the start node to the goal node, this path

must be the optimal one (given some criteria of

optimality).

• Completeness: Given that the goal is reachable,

the algorithm must reach it in a ﬁnite time.

If the path planner we deal with veriﬁes those proper-

ties, we know for sure that a path will eventually be

found (given that the goal node is reachable) and that

this path will be the best one, according to a given

criteria.

2.2 Spaces Used

We want to ﬁnd a path in a n-dimension con-

ﬁguration space

X . The n dimensions represent

the degree of freedom (dof) of the system. We

have to deal with two subspaces of X , one repre-

senting the free-conﬁguration space (called

free

where we can move, and the other representing the

collision-conﬁguration space (called

collision

), where

the movements are impossible.

2.3 Structure of the Graph

We discretize

free

into an inﬁnite number of nodes.

Let

N be the set of those nodes. They are char-

acterised by n parameters. We can build a directed

graph (the nodes are linked together by one-way

edges) in

free

, which follows those properties :

Property 1 There is a minimum positive edge cost

between any two nodes (i.e., there is a given positive

ε so that the cost between any two nodes is greater

than ε)

Property 2 The number of edges starting from a

node is ﬁnite.

Property 3 The edge cost between any two nodes in

a path is ﬁnite.

When a graph possesses the ﬁrst two properties,

it is called a locally ﬁnite graph (in a given and ﬁnite

area of the space, there is a ﬁnite number of paths

between any two nodes) (Nilsson, 1988).

Algorithm 1 A*

1: CLOSE ←

2: OPEN ← NodeStart

3: while OPEN6=

0 do

4: Node ← Shortest

∗

Path(OPEN)

5: CLOSE ← CLOSE + Node

6: OPEN ← OPEN - Node

7: if Node = NodeGoal then

8: return (Success,Node)

9: end if

10: NEWNODES ← Successors(Node) /∈ CLOSE

11: for all NewNode of NEWNODES do

12: if NewNode /∈ OPEN or g(NewNode) >

g(Node)+cost(Node,NewNode) then

13: g(NewNode) ← g(Node)+cost(Node,NewNode)

14: f

∗

(NewNode) ←

g(NewNode)+h(NewNode,NodeGoal)

15: parent(NewNode) ← Node

16: if NewNode /∈ OPEN then

17: OPEN ← OPEN+NewNode

18: end if

19: end if

20: end for

21: end while

22: return NoSolution

2.4 Paths of the Graph

• let

P be the set of all the paths beginning at the

start node and exploring the conﬁguration space.

An important characteristic is that no path in P

can loop (a path cannot go through a node twice).

• let P

goal

be the set of all the paths beginning at the

start node and ending on the goal node.

• let

goal

be the set of all the paths beginning at the

start node and that do not reach the goal node. We

have

goal

P \P

goal

. (1)

An important thing to keep in mind is that the A*

works incrementally with

P . It tests paths of P node

by node and does not know if those paths belong to

goal

until it reaches their ﬁnal node.

2.5 Parameters of the Algorithm

2.5.1 Criteria of Optimality

Each path has an associated cost, which is the cumu-

lated costs of every edge in the path. Let f(c

) be this

cost, for all c

∈

P .

Let us introduce, g(c

,n), which represents the

length of the part of the path c

going from the start

node to the node n, n ∈ c

. If c

is ﬁnite, we can ob-

serve that

f(c

) = g(c

,ﬁnalnode(c

)). (2)

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

104

We want to ﬁnd a shortest path, i.e. a path c ∈ P

goal

that verify

f(c) ≤ f(c

) ∀c

∈

goal

. (3)

Finding such a path is our criteria of optimality.

2.5.2 Rule to Expand the Graph

As it is well known, the A* works with a path-

independant heuristic function h(n

) that estimates

the unknown length between two nodes n

and n

. We

can then work with an estimated total length f

∗

,n)

rather than the real (unknown) f(c

), such that

∗

,n) = g(c

,n) + h(n, goalnode). (4)

2.6 Proofs of the Algorithm

Both the proof of optimality and of completeness have

been given in (Nilsson, 1988). We won’t detail them

further here.

3 THE A* IN AN UNCERTAIN

CONFIGURATION-SPACE

3.1 The Concept of Uncertainties

The uncertainties represent the fact that in the real

world, one cannot precisely know the real model of

the systems. Thus, we can determine the errors in-

duced by all the models used and we can calculate the

uncertainties associated to each node (in

N ) of each

path (in

P ). Let us consider that the uncertainties

are fully described in a m-dimension space. Conse-

quently, a full uncertain conﬁguration is now given by

n+ m parameters: n for the coordinates in the space

X and m for the coordinates in the uncertainties-

space. Such an uncertain conﬁguration will be called

an extended node.

In this section, we are going to present a way to run a

simple A* on a new graph based on extended nodes,

in considering that the uncertainties can be managed

as additional degree of freedom.

3.2 Spaces Used

Let

be the uncertain conﬁguration space, which

is {n+ m}-dimension space and fully include

X (we

can then consider that we deal with {n+m} dof, even

if some of them are uncertainties). Knowing the un-

certainties associated to a given node, we can build a

new n+ m dimensions free-conﬁguration space X

free

taking into account those incertainties.

Let

collision

be characterised by

collision

free

. (5)

Our goal is now to ﬁnd a safe path, which is a path that

never go in

collision

(considering the uncertainties of

every extended node).

3.3 Structure of the Graph

In the previous case (see section 2.3 above) we al-

ready introduced a graph based on

N . However, we

do not want to work with the set of nodes

N , as it

does not take the uncertainties into account. Let us

introduce a new set of nodes, called

U . The nodes

in this set will then be extended nodes. The proper-

ties applying on N apply also on U (the three proper-

ties presented in section 2.3 applie on this new graph).

Thus, our new graph is locally ﬁnite.

Another property may be pointed out:

Property 4 An inﬁnite number of extended node can

be based on the same node.

3.4 Paths of the Graph

We can now introduce the paths generated through the

extended nodes of

U .

• let

G be the set of all the paths beginning at the

start extended node and exploring the {n + m}-

dimension space conﬁguration

free

. The criteria

of optimality presented in section 2.5 applies fully

here.

• let

goal

be the set of all the paths beginning at

the start extended node and reaching the goal ex-

tended node. A direct consequence is that

goal

⊂

G .

• let G

goal

be the set of all the paths beginning at the

start extended node and that do not reach the goal

extended node. We have

goal

G \G

goal

. (6)

• g, f, f

∗

and h represent respectively the length

from the start node to a ﬁxed node in a path, the

total length of a path, the estimated length of a

path and the heuristic of a path in the {n + m}-

dimension space.

What we ﬁnally search is the shortest path, which

is the shortest path c in

G . This path must verify

f(c) ≤ f(c

) ∀c

∈

G . (7)

MINIMUM COST PATH SEARCH IN AN UNCERTAIN-CONFIGURATION SPACE

105

3.5 The Algorithm

It is now possible to implement the A*, which we

will call A* in an Uncertain Conﬁguration-Space

(AUCS*), directly on the newly described {n+ m}-

dimension graph. The AUCS* is exactly the same al-

gorithm than the A*, the only difference is the graph

on which it performs. The goal node is then deﬁned

by n+ m parameters, the ﬁrst n corresponding to the

location we want to reach and the m other parameters

being the uncertainties we want to have at this loca-

tion.

As the algorithm is exactly the same (despite it per-

forms on a different graph), we do not give it again.

See algorithm 1.

3.6 Proofs of the Algorithm

3.6.1 Proofs of Optimality

Despite the algorithm performs on an improved

graph, its principal characteritics are unchanged. The

heuristic and calculus of length have the same proper-

ties (g and h could eventually be identical to those

used in the previous section), and regarding this

model the AUCS* still ﬁnd the shortest path. 

3.6.2 Proofs of Completeness

There again, the only cases the AUCS* could not be

complete is when there is an inﬁnite number of paths

such as their lengths are lower than the found path’s

or when a path with a length lower than the found path

is inﬁnite.

The proof is equivalent to the one determined by

(Nilsson, 1988): the properties of the graph built are

the same than in the previous case (it is locally ﬁnite),

this is enough to lead to the same conclusion ; the

AUCS* algorithm is complete. 

3.7 Working in a Finite Graph

In the previous sections, we considered the graphs

were inﬁnite and built in advance, before the algo-

rithm starts. However, building an inﬁnite graph

is impossible for a (necessarily limited) computer.

Thus, some choices must be done if we want to be

able to run the algorithm on a computer. For exam-

ple, we could deﬁne a sub-space of

X where to run the

algorithm. Of course, this implies no path not being

entirely contained in

X can be found. This is gener-

ally not a problem if the sub-space is cleverly deﬁned

(for example, if we want to ﬁnd a path leading from

a room to another in a building, we could restrain our

space to the building itself). This is enough to ensure

the new graph will be ﬁnite:

Property 5 A graph generated in a ﬁnite sub-space

X and following properties 1 and 2 (section 2.3) is

ﬁnite.

Proof: Property 1 ensures that any two nodes must

have a given minimum positive edge cost. Conse-

quently, a ﬁnite sub-space can only be ﬁlled with a

ﬁnite number of nodes. Property 2 ensures that only a

ﬁnite numbers of edges can go from a node: the graph

is ﬁnite. 

Thus, we can work on a pre-built sub-graph of

X .

However in the case of

, the problem is slightly

different. Property 4 implies there could be an inﬁnite

number of extended nodes on each node. Of course, it

is not possible to build such a graph with a computer.

Thus, we are going to work on a dynamically built

graph: from

P , the extended nodes are dynamically

added in the new graph

G when they are reached.

Proofs of optimality and completeness ensure that the

graph stays ﬁnite, as the dynamically building of the

extended nodes ends when the algorithm reaches the

goal node, which has been proven happens in a ﬁnite

time.

The only change in algorithm 1 is that line 10, Suc-

cessors function dynamically creates the nodes if they

do not already exist. This implies that, given a fully-

described extended node and the base of the node

where to go, Successors must be able to calculate the

m last parameters of the node to reach.

We now have an implementable algorithm in an un-

certain conﬁguration space, which does not need inﬁ-

nite memory.

4 THE A* WITH TOWERS OF

UNCERTAINTIES

4.1 The Towers of Uncertainties

In section 3.7, we showed that we could work on a

ﬁnite dynamically built graph. However, ﬁnding a

way to reduce the need of memory needed would be

very helpful. (Lambert and Gruyer, 2003) has deter-

mined a method (described below) that reduces the

ﬁeld of search (reducing the memory needed) and re-

organizing the way to record the data. This method

involves the use of towers of uncertainties, and only

works on dynamically built graphs (as presented sec-

tion 3).

However, the graphs must be more constrained than

those seen in the sections above.

Constraint 1: This constraint concerns the evolution

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

106

of the uncertainties between two nodes: this evolu-

tion follows a given function and this function is the

same for every uncertainty. If the uncertainties of a

given extended node include the uncertainties of an-

other extended node (those extended nodes must be

based on the same node), then the pattern ensures that

this property will be kept in the following nodes of the

paths (when the paths follow the exact same nodes).

Constraint 2: If the function (collision-detection) giv-

ing the conﬁguration-space where the path belongs

(

free

collision

) declares the path in

collision

for

a given uncertainty, then it will declare a new path

following the same (non-extended) nodes with wider

uncertainties in

collision

too.

When the AUCS* searches the graph, it generates

various beginning of paths. Given that numerous ex-

tended nodes can be at the same position (see prop-

erty 4 above), it is perfectly possible to have two or

more different paths reaching the same position (same

node, but different extended nodes).

Without knowing what comes in the future, could not

we already discard some of those paths?

The criteria of optimality we want to minimize is the

length of the path. What we search is the smallest

path. However, some paths expanded at some step

of the algorithm may not reach the goal (for exam-

ple, every path built from this path does not belong

free

). Thus, the current smallest path could be in

collision

without the algorithm knowing it (the part

being in collision with the environment having not

been reached by the algorithm yet) and a longer path

could be in

free

. The immediate consequence is that

we should keep both of them during the search.

What about if the longer path has also wider uncer-

tainties? Then it is clear that if the smallest path does

belong to

goal

, the longer path belongs to

goal

too

(this is ensured by constraint 2 presented above, in

section 4.1, as from this node on, the exact same paths

will be tried by both of the paths). In this case, we

could discard the longer path. On the other hand, if

the smallest path is in

free

, then we do not need the

longer path anymore, even if it is in

free

too, as its

length is by deﬁnition longer. We could also discard

it in this case.

Consequently, we can always discard the longer path

with the wide uncertainties.

This property considerably reduces the complexity of

the algorithm, as it detects earlier useless paths. In

order to make the detection of the useless paths the

quicker possible, (Lambert and Gruyer, 2003) pro-

poses the use of towers of uncertainties in an algo-

rithm we call SATU* (Safe A* with Towers of Un-

certainties).

Each extended node is divided in two entities:

• A base : Given by the node of the extended node

(the n ﬁrst parameters).

• A level : The uncertainties associated to the node

and function of the path leading up there (the m

following parameters).

So, we can now dynamically (while the algorithm per-

forms) build a tower of uncertainties, with a common

base, and as many levels as the number of extended

nodes (with the same common base) opened at this

point. The levels are placed following some rules:

• The levels are placed in an increasing order of

their length.

• Given a level, if another level under it has uncer-

tainties that completely ﬁts in its own, then the

given level is removed (its length is bigger and its

uncertainties are wider than the level’s under it).

A direct consequence of the description above is

that the memory needed for the SATU* is lower than

for the AUCS*, even if we consider that the AUCS*

build the graph dynamically too: for a given number

n of extended nodes which have the same base, the

needed memory will be (n+ m)n in the AUCS* and

only n+ n.m in the the SATU*.

4.2 Description of the Algorithm

SATU* is given in algorithm 2. The ﬁrst part of the

algorithm is the same as algorithm 1: two lists are cre-

ated and initialized, a loop allows to expand extended

node after extended node (selecting the extended node

with the smallest f), a test verify if the goal extended

node has been reached, otherwise the successors of

the current extended node are selected and opened.

For each of those successors, a ﬁrst test (line 12)

checks if the base of the successor is already in

CLOSE or OPEN. If it is not, then when can create

a new tower on this base, add a ﬁrst level with the un-

certainties associated with the successor and add the

base of the node in OPEN (without forgetting to cal-

culate f and to store the parent of the node in order

to be able to ﬁnd the path when the algorithm is ﬁn-

ished).

If the base of the successor belongs either to CLOSE

or OPEN, we need to check if the successor may enter

the tower or not.

In order to do that, the algorithm must compare each

one of the levels of the tower with the successor’s

uncertainties and g. Line 20 and 21, the algorithm

selects the tower and initialized an index which will

represent the value of the current level being checked.

Line 22 begins the loop that will compare the current

level extracted and the successor.

Line 25, the current level to compare is extracted from

MINIMUM COST PATH SEARCH IN AN UNCERTAIN-CONFIGURATION SPACE

107

Algorithm 2 (SATU*)

1: CLOSE ←

2: OPEN ← NodeStart

3: while OPEN6=

0 do

4: Node ← Shortest

∗

Path(OPEN)

5: CLOSE ← CLOSE + Node

6: OPEN ← OPEN - Node

7: if Base(Node) = Base(NodeGoal) and uncer-

tainty(Node) ⊆ uncertainty(NodeGoal) then

8: return Success

9: end if

10: NEWNODES ← Successors(Node)

11: for all NewNode of NEWNODES do

12: if NewNode /∈ OPEN,CLOSE then

13: g(NewNode) ← g(Node)+cost(Node,NewNode)

14: f

∗

(NewNode) ←

g(NewNode)+h(NewNode,NodeGoal)

15: build(NEWTOWER,base(NewNode))

16: AddLevel(NEWTOWER,NewNode)

17: OPEN ← OPEN+NewNode

18: parent(NewNode) ← Node

19: else

20: TOWER ← ExtractTower(base(NewNode))

21: level ← -1

22: do

23: AddLevel ← false

24: level ← level+1

25: LevelNode ← ExtractNode(TOWER,level)

26: if (g(NewNode) ≥ g(LevelNode) and uncer-

tainty(LevelNode) * uncertainty(NewNode)) or

g(NewNode) < g(LevelNode) then

27: AddLevel ← true

28: end if

29: while level 6= TopLevel(TOWER) and Ad-

dLevel=true

30: if AddLevel=true then

31: level ← insert(NewNode,TOWER)

32: OPEN ← OPEN + NewNode

33: parent(NewNode) ← Node

34: UpperNodes ←

nodes(UpperLevels(TOWER,level))

35: for all uppernode of UpperNodes do

36: if uncertainty(NewNode) ⊆ uncer-

tainty(uppernode) then

37: remove(TOWER,uppernode)

38: OPEN ← OPEN - uppernode

39: end if

40: end for

41: end if

42: end if

43: end for

44: end while

45: return NoSolution

the tower.

Line 26, a test is performed. We may insert the suc-

cessor in the tower if either its g is greater or equal

than the level’s and its uncertainties are not included

in the level’s or if its g is lower than the level’s. Thus,

if those conditions are met, we keep the successor and

try to compare it to the next level. If, just once, those

conditions are not met, then we can discard the suc-

cessor.

Line 30, if we may insert the successor in the tower,

we do it.

Line 31, we insert the successor in the tower in the

increasing order of level’s g.

we then add the successor in OPEN, for it to be ex-

panded later.

Line 34, we select the nodes with a greater g than the

successor’s, and check if they can be kept (if their un-

certainties are included in the successor’s, then we can

discard them). This is done in removing them from

the tower and from OPEN lines 37 and 38.

4.3 Proofs of the Algorithm

4.3.1 Proof of Optimality

As shown in section 4, the SATU* works as a AUCS*.

The addition of the tower of uncertainties only allows

to discard very early a path that is either in

goal

not the smallest path in

free

. Thus, regarding the dis-

covery of the smallest path, it has the exact same be-

haviour than a AUCS*, which proves that it respects

the criteria of optimality. The algorithm therefore en-

sures that its found path c veriﬁes

g(c) ≤ g(c

) ∀c

∈

free

. (8)



4.3.2 Proof of Completeness

Given that the optimality of the SATU* has been

proven above and that the graphs used have the same

properties than for the AUCS*, we can directly con-

clude that the SATU* is complete (as the graphs used

are locally ﬁnite). 

5 CONCLUSION

In this paper, we ﬁrstly presented a simple way to run

an A* algorithm in an uncertain-conﬁguration space

(AUCS*) by considering the uncertainties as simple

degree of freedom of the system. This characteristic

allows the add of uncertainties in any path planner al-

gorithm that respects the necessary properties.

Secondly, we introduced a new path planner,

the SATU* algorithm, working in an uncertain-

conﬁguration space, strongly based on the A* algo-

rithm that limits the memory needed. We then gave

the needed proofs of optimality and completeness as-

sociated. Some further work will focalize on the cal-

culation and comparison of the AUCS* and SATU*

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

108

complexities and on the experimental tests using a

real mobile robot.

REFERENCES

Dijsktra, E. (1959). A note on two problems in connexion

with graphs. In Numerische Mathematik, volume 1,

pages 269–271.

Fraichard, T. and Mermond, R. (May 1998). Path planning

with uncertainty for car-like robots. In IEEE Int. Conf.

on Robotic and Automation, pages 27–32.

Hart, P., Nilsson, N., and Raphael, B. (1968). A formal

basis for the heuristic determination of minimum cost

paths. In IEEE Transaction on Systems Science and

Cybernetics, SSC-4(2), pages 100–107.

Lambert, A. and Gruyer, D. (September 14-19, 2003). Safe

path planning in an uncertain-conﬁguration space. In

IEEE International Conference on Robotics and Au-

tomation, pages 4185–4190.

Latombe, J. C. (1991). Robot Motion Planning. Kluwer

Academic Publisher.

Lazanas, A. and Latombe, J. C. (1995). Motion planning

with uncertainty : a landmark approach. In Artiﬁcial

Intelligence 76(1-2), pages 287–317.

Nilsson, N. J. (1988). Principles of artiﬁcial intelligence.

Morgan Kaufman Publisher inc.

Pepy, R. and Lambert, A. (2006). Safe path planning

in an uncertain-conﬁguration space using rrt. In

Proc. IEEE/RSJ International Conference on Intelli-

gent Robots and Systems, pages 5376–5381, Beijing,

China.

Russel, S. J. and Norvig, P. (1995). Artiﬁcial Intelligence: A

modern Approach, pages 92–101. Prentice Hall, Inc.

MINIMUM COST PATH SEARCH IN AN UNCERTAIN-CONFIGURATION SPACE

109