In Proc. 2003 IEEE Int’l Conf. on Humanoid Robotics (Humanoids 2003)

Planning Biped Navigation Strategies inComplex Environments
Joel Chestnutt1 , James Kuﬀner1,3 , Koichi Nishiwaki2 , and Satoshi Kagami3
1

Robotics Institute, School of Computer Science, Carnegie Mellon University, USA
2

3

School of Information Science and Technology, University of Tokyo, Japan

National Institute of Advanced Industrial Science and Technology (AIST), Japan

Abstract. We present an algorithm for planning goal-directed footstep
navigation strategies for biped robots through obstacle-ﬁlled environments and uneven ground. Planning footsteps is more general than mostexisting navigation methods designed for wheeled robots, since the options of stepping over or upon obstacles are available. Given a height
map of the terrain and a discrete set of possible footstep motions, theplanner uses an A* search to generate a sequence of footstep locations
to reach a given goal state. The planner evaluates footstep locations forviability using a collection of heuristic metrics designed to encode therelative safety, eﬀort required, and overall motion complexity. We show
preliminary results of the planner over several simulated terrains, as well
as a simpliﬁed, online version of the algorithm running on the H7 humanoid robot. In the latter case, a stereo vision system is used to sense
obstacles in the immediate environment and identify a target goal location, which is used to update the current optimal footstep sequence tothe goal from the robot’s present location.

Fig. 1. Example footstep navigation strategies for complex scenes output by our planner (left: a room with spikes. right: two platforms connected by stairs.) The lighter endof each footstep signiﬁes the front of the foot.

1

Introduction

As humanoid robots continue to advance and gain new capabilities, software
for high-level control and autonomous motion generation will be required. One
important area of research involves the design of algorithms to compute robustnavigation strategies for humanoids in complex human environments. For indoor
environments, this includes dealing with furniture, walls, stairs, doors, and possible objects on the ﬂoor. For outdoor environments, this includes the ability tonavigate on rough terrain and uneven surfaces. Because legged robots have theability to step over and onto obstacles in their path, they are uniquely suited toovercoming these diﬃculties.
The planner described in this paper allows the humanoid to take advantage
of its bipedal capabilities and navigate through diﬃcult environments where conventional 2D planning algorithms designed for wheeled robots would be unable
to ﬁnd a solution. Heuristics designed to minimize the number and complexityof the step motions are used to encode cost functions used for searching a footstep transition graph. If successful, the planner returns an optimal sequence offootstep locations according to the cost functions and plausible sets of footstep
locations deﬁned.
The rest of the paper is organized as follows: Section 2 gives an overview ofrelated research, Section 3 describes our biped stepping model and the planningalgorithm, Section 4 shows some results from both simulation and an online
implementation using the humanoid H7, and Section 5 concludes with a summary
discussion and directions for future research.

2

Background

Patla and colleagues have studied the problem of how humans perform local planning over irregular terrain based on visual feedback[PAM+ 96, Pat98, PNS00].
For humanoid robots, most existing research has focused on pre-generating stable walking trajectories (e.g. [HHHT98, YINT98, NII99]), or on dynamic balanceand control (e.g. [VBSS90, PP99]). Recently, techniques have been developed togenerate stable walking trajectories online [NSK+ 01, kNa], though these results
do not account for obstacles. For quadruped robots, adaptive gait generationand control on irregular terrain and among obstacles has been previously studied [Hir84]. This method has not yet been applied to biped robots. Sensor-based
obstacle-avoidance techniques have been developed for bipeds navigating in unknown environments [YL99, LDS+ 00]. However, such reactive methods can become trapped in local loops or dead-ends, since they do not consider global
information. Other related techniques in computer animation that use footprint
placement for motion speciﬁcation have been developed for bipeds[Gir87, vdP97],
and quadrupeds[KMB95, TvdP98]. We previously implemented a global planning approach for bipeds that generated statically-stable walking motions fora simulated H6 robot in cluttered 2D environments [KNK+ ]. In this paper, we
handle complex height maps, uneven terrain, and allow stepping motions thatare not statically-stable. The most complex examples still run only in simulation,
but we have created a version of the planner that uses a simpliﬁed robot walking model and obstacle collision checking routine to reduce the computationalcost. Our current implementation is fast enough to enable continuous replanningduring each step cycle, and has been run successfully on a real robot.
Our approach is to build a search tree from a discrete set of feasible footstep
locations corresponding to candidate stable stepping motion trajectories. Using
standard dynamic programming techniques, we compute a sequence of footstep
placements to reach a goal region in an environment with obstacles. This has
the advantage of planning a global navigation strategy for bipeds that includes
the ability to step over obstacles. Encoded heuristics minimize the number andcomplexity of the steps taken, as well as guide the search. Given a proper admissable heuristic cost function, the planner can compute optimal sequences offootstep locations to reach a goal, according to the cost function deﬁned. For thesimpliﬁed online version, this strategy can be computed eﬃciently on standard
PC hardware (usually in a fraction of a second for paths involving less than
20 steps to reach the goal). We have connected this version to an online stereo
vision system and walking trajectory generator for the humanoid robot H7. The
robot continuously updates a walking area map computed by the stereo visionsystem, and replans an optimal footstep sequence to reach the current goal.

3

Algorithm

The planner takes as input a map representing the terrain to plan over, an initial
and goal state, and a discrete set of possible footsteps that can be taken. If apath is found the planner returns the solution as an ordered list of the footsteps
that should be taken to reach the goal.
Biped Robot Model: The robot capabilities are modeled by storing a symmetric collection of candidate footstep transitions for each foot. The footsteps are
represented as a displacement from the robot’s current state, which consists of
Q = (x, y, θ, s). The state variables x, y, and θ denote the relative position andorientation of the footstep, and the binary variable s ∈ {R, L} denotes which footis currently the support foot (right or left). Four examples of footstep transition
sets that we used in our experiments are illustrated in Figure 2.
State Transitions: Each footstep transition T = (Q, cost, HCupper , HClower , clearance)
consists of a relative candidate footstep displacement Q, an associated cost, an
upper and lower allowable height change HCupper and HClower , and an obstacle
clearance value, clearance, representing the largest obstacle that the transition
can step over. Each transition is assumed to be independent of all previous transitions and the robot’s current state. Transition displacements, height change
allowances, and obstacle clearances are chosen manually based on the actual
robot’s capabilities in a way so as to cover the robot’s range of motion as well
as desired. The associated costs are chosen manually to have the planner prefercertain transitions over others.

8 transitions

16 transitions

26 transitions

40 transitions

Fig. 2. Footstep transition sets. The transitions displayed are only those for the leftfoot (relative to the right foot shown in black).

Environment: The terrain map M is represented by a grid of cells. Each cellc = (x, y, height, inf o) has an associated location in the grid (x, y), a height value
height, and an information value inf o. Together, the cells create a height map
representing the form of the terrain the planner must overcome. The information
values provide extra knowledge of the terrain, such as places which appear to besafe to step on but should be considered unsafe.
State Evaluation: The planner evaluates all transitions from a state, generating
three costs for each one. First is the location cost L(Q), which evaluates thetransition’s destination as a potential foothold. This cost uses a variety of metrics
to quickly compute how viable a location is for stepping to. Second is a step
cost S(Q, T, Qc ), which computes the cost of reaching the state Q by making
the transition T from the current state Qc . This cost includes the transition’s
associated cost, a penalty for height changes, as well as an obstacle clearance
check of the terrain between the foot’s last position an the new foothold. Finally,
the third cost is a heuristic R(Q, Qg ) which estimates the remaining cost to reachthe goal state, based on the distance to the goal, the angle of the robot to thegoal, and the height of the robot relative to the goal. These costs can thenbe used by the planner in one of several diﬀerent algorithms: best-ﬁrst search,
A* search, breadth-ﬁrst search, dynamic programming, etc. Note also that thelocation cost is independent of the transition or current state of the biped, andthus can be precomputed for all states if desired, rather than computed for theneeded states at run-time.
3.1

Location Metrics

To evaluate a location’s cost, we would like to know exactly how the foot would
come to rest on the surface, which parts of the foot would be supported, andbe able to build a support polygon of the foot based on which parts of thefoot are touching the ground and evaluate how stable that support polygon is.
Unfortunately, this is very expensive to exactly compute. Instead, we use a setof metrics which can be quickly computed and serve to approximate this ideal

location cost. To be useful, a metric should be quick to compute, invariant to
the number of cells at the location, and should eliminate/penalize an unwanted
form of terrain while not eliminating or heavily penalizing good terrain.
To compute the metrics, we ﬁrst determine the relevant cells to include. Basedon the foot’s positions and orientation, we test every cell in the rectangular box
around it. We rotate these cells’ coordinates into the foot’s coordinate systemand then test them against the foot’s shape. This gives us a set of cells, C, to
use with each of the metrics deﬁned below:

The slope angle of the surface at the candidate location.
Perfectly horizontal surfaces are desired. The slope angle
is computed by ﬁtting a plane h(x, y) to the cells in thelocation.
The ”roughness” of the location. A measure of the deviationof the surface from the ﬁtted plane. Computed by averaging
the diﬀerence in height of each cell to the plane’s height at
the that cell.
c∈C
1 |c.height − h(c.x, c, y)|
N c

(1)

The ”stability” of the location. This metric evaluates thecurvature of the location. While perfectly ﬂat is desired,curving down at the edges is penalized more than curving
up at the edges. This metric is computed using a weighted
sum of the heights (convolving with a dome-shaped ﬁlter).
c∈C
1 {[c.height − h(c.x, c.y)]g(c.x, c.y)}
N c

(2)

g(x, y) is the dome-shaped ﬁlter. Restrictions on g(x, y) arethat it should be higher in the center than on the edges,
and it should sum to zero over the area of the foot. The
ﬁlter we used (in the foot’s coordinates) was:g(x, y) = cos(

2πy2πx) + cos()
wlw and l are the length and width of the foot.

(3)

The largest bump of the location. Bumps above the ﬁtted
plane are much worse for a location than holes in the plane.
This metric ﬁnds the largest deviation above the plane.
max{c.height − h(c.x, c.y))|c ∈ C}

(4)The ”safety” of the location. This refers to the area aroundthe location. It’s purpose is to take into account the possible
inaccuracy of foot positioning. This can be computed using
the roughness and largest bump metrics, using the cells
around the foot location.

Each of the metrics has both a weight and a cutoﬀ value associated with it.
If any metric passes its cutoﬀ value, the location is discarded. Otherwise, thelocation’s cost is the weighted sum of these metrics.
3.2

Step Cost

The cost of taking a step is given by:
S(Q, T, Qc ) = T.cost + wh |H(Q, Qc )|(5)T.cost is the cost of the transition T . H(Q, Qc ) is the height change between
the state Q and the current state of the robot Qc . wh is the penalty for heightchanges, chosen manually based on the user’s desire to try to avoid paths thatgo up or down. If the height change is outside the transition’s allowable range,
the step is discarded. An obstacle collision check is also done to determine if thefoot can safely clear all the cells along the path from its previous location to its
new location. Because this path may include many cells, quadtrees that encode
the maximum height are used to quickly check for collisions (see Figure 3). If
collisions are found, the step is discarded.
3.3

Estimated Cost Heuristic

The estimated remaining cost to reach the goal is calculated by:
R(Q) = wd D(Q, Qg ) + wθ |Θ(Q, Qg )| + wh |H(Q, Qg )|(6)

D(Q, Qg ) is the euclidean distance from the state Q to the goal state Qg .
Θ(Q, Qg ) is the relative angle of the robot in state Q to the goal. H(Q, Qg )is the diﬀerence in height between the state Q and the goal state. The weights
wd and wθ are chosen based on the costs associated with the transitions. wh isthe penalty for height changes applied during step cost calculations.

Fig. 3. An example quadtree constructed so that the value of a cell is the maximum
value of its four children on the next level. This allows large amounts of the terrain toquickly be eliminated when searching for obstacle collisions.

Fig. 4. Cluttered terrain with obstacles that can be stepped over (left), stepped onto
(center), or large obstacles that must be stepped around (right)

4

Results

This section presents some preliminary results using prototype planners runningin both a simulation environment, and on the humanoid ’H7’. The simulation
results were computed on a 1.8 GHz Pentium4 PC with 1GB RAM running
RedHat Linux 7.1. The planner was able to ﬁnd suitable paths for a wide variety
of terrain types.
Figure 4 shows examples involving cluttered terrain with obstacles that canbe stepped over, stepped onto, or large obstacles that must be stepped around.
Figure 5 illustrates two diﬀerent multi-level terrains with planned footsteps,
including an environment with clear stairs, and an environment with obstacleﬁlled stairs and platforms of varying heights. The two examples in Figure 6 show
the planned output over terrain with obstacles on uneven ground.
4.1

Distance to Goal

Over ﬂat terrain, the running time of best-ﬁrst search scales linearly with the
distance to goal. A* and truncated A* also scale roughly linearly with distanceto goal. Figure 7 compares the performance of A* and BFS for walking towards
a distant goal located at various distances along a forward diagonal line. The
ﬂuctuations in the total calculation times are due to the goal state falling in
between two preferred footstep transitions.

Fig. 5. Multi-level terrain with footsteps planned over clear stairs (left), obstacle-ﬁlled
stairs (center), and platforms of varying heights.

Fig. 6. Planning over terrain with obstacles on uneven ground.

Varying Distance From Goal Location
Best First Results

6
0.07

Best First Search
A* Search
Truncated A* Search

5

0.06

0.05
Planning Time (s)Planning Time (s)

4

3

0.04

2

0.03

1

0.02

0

0

0.5

1

1.5
2
Distance to Goal Location (m)

2.5

3

0.01

0

0.5

1

1.5
2
Distance to Goal Location (m)2.5

3Fig. 7. Performance comparison of A* and BFS for walking towards a distant goal
located at various distances along a forward diagonal line.

Best-ﬁrst SearchA*Best−First Search with 40 Transitions

A* Search with 40 Transitions

0.3

100

0.2

Planning Time

Planning Time

0.25

0.15
0.1
0.05

80
60
40
20

0

0

30

30

5
25
20

5
25

10

10
20

15

15
15

15
20

10

Goal y location30
Goal x location

A* Search with 26 Transitions

Best−First Search with 26 Transitions

80

0.3

Planning Time

0.25
Planning Time25

5Goal y location30

Goal x location

10

25

5

20

0.2
0.15
0.1

60
40
20

0.05

0

0
30

5
25

10

30

5
25

10
20

20

15

15

15

15
20

10

Goal x location

10

25

5
30

20
25

5

Goal y location

Goal x location30

Goal y location

Fig. 8. Performance comparison of BFS (left) and A* (right) for diﬀerent sets of available footstep transitions. Each row shows plots of the total computation time requiredfor various goal locations for a robot that can step forwards, sideways, and backwards(top row, transitions=40 ), and for a robot that can only step forwards and sideways(bottom row, transitions=26 ). The initial state of the robot is at location (10,10) in
each graph (as indicated by the small arrow), facing along the positive y-axis.

The plots in Figure 8 compare the performance of BFS (left) and A* (right)
for diﬀerent sets of available footstep transitions. Each row shows plots of thetotal computation time required for various goal locations for a robot that canstep forwards, sideways, and backwards (top row, transitions=40 ), and for arobot that can only step forwards and sideways (bottom row, transitions=26 ).
The initial state of the robot is at location (10,10) in each graph (as indicatedby the small arrow), facing along the positive y-axis. Interestingly, the regions
to the back and right relative to the robot’s current location can be planned
signiﬁcantly faster using the forward-only transitions. The reason for this diﬀerence appears to arise from having fewer movement options that actually makeprogress towards a goal at that location, whereas having more movement options
causes the planner to expand many more relatively equal-cost paths.
4.2

Transitions and Obstacle Eﬀects

A* Search

Best−First Search

40
0.07

35
40 Transitions
26 Transitions
16 Transitions
8 Transitions

30

0.06

0.05

Time (s)Time (s)

25

20

0.04

15

0.03

10

0.02

5

0.01

0

0

1

2
Number of Stairs in Path

3

4

0

40 Transitions
26 Transitions
16 Transitions
8 Transitions0

1

2
Number of Stairs in Path

3

4

Fig. 9. Performance comparison of A* (left) and BFS (right) for increasing numbers ofstairs along the path from the initial to goal state. The transition set containing only
8 transitions is fails to ﬁnd a path as soon as the ﬁrst step is added.

In general, decreasing transitions decreases runtime, since the branching factor is smaller. However, this smaller number of options decreases the set ofsolvable maps. The example in Figure 9 compares the performance of A* (left)
and BFS (right) for increasing numbers of stairs along the path from the initial
to goal state. Here, the smallest number of transitions was unable to get past
the ﬁrst stair, and could only solve the empty map.
In the presence of local minima, Best-ﬁrst search can increase signiﬁcantly
the running time and often generates highly undesirable paths. Figure 10 shows acomparison of the output of BFS versus A* on environments with local minima.
In the example with deep local minima, A* search outperforms Best-ﬁrst in
running time as well as producing optimal footstep paths.

Best-ﬁrst Search

A*

Fig. 10. Comparison of the output of BFS versus A* on environments with local minima. With deep local minima (bottom), A* search outperforms Best-ﬁrst in running
time as well as optimality.

4.3

Metric Weights

Effect of Heuristic Metric Weights
60

50

Time (s)

40

30

20

10

0

0

1

2

3
4
5
6
7
Magnitude of Weights for Heuristic Metrics

8

9

10

Fig. 11. Performance eﬀect of scaling the weights used for the heuristic metrics. Magnitude zero corresponds to zero weights. Magnitude 1 corresponds to the manuallychosen weights for each metric.

By scaling the weights of the metrics applied to each location, we can see thatthey bring about an exponential increase in the running-time of the algorithm.
The increase in the weight of the location cost encourages the planner to try
more locations, in the hope of ﬁnding a more optimal path. Figure 11 showsthat carefully choosing the weights for the diﬀerent metrics is very important
for the runtime of the algorithm. In many cases, it may be better to simply use
the metrics for discarding locations via their cutoﬀ values, and lower the metricweights to zero.
4.4

Online Footstep Planning Experiments

We have developed a simpliﬁed, online version of the algorithm running on theH7 humanoid robot. A stereo vision system is used to sense obstacles in theimmediate environment and identify a target goal location, which is used to
update the current optimal footstep sequence to the goal from the robot’s present
location. This strategy can be computed eﬃciently on standard PC hardware(usually in a fraction of a second for paths involving less than 20 steps to reachthe goal). The output of the footstep planner is sent to a walking trajectory
generator for the humanoid robot H7. The robot continuously updates a walking

Fig. 12. Vision Processing steps. (top row:) raw camera images and resulting 3D
Depthmap; (bottom row:) mesh model, planar surface identiﬁcation, and ﬁnal walking area map.

Fig. 13. Online footstep planning: (top row : H7 humanoid and candidate walking surface area extracted from 3D stereo depth map; bottom row : distortion-corrected obstacle map after thresholding, and calculated optimal footstep sequence to target.

Fig. 14. Snapshots of the humanoid H7 calculating and executing online the footstep
sequence from Figure 13

area map computed by the stereo vision system, and replans an optimal footstep
sequence to reach the current goal.
Figure 12 shows the output of the 3D vision system. The vision information
is used to calculate the walking area map, which is used as input to the planner.
Figure 13 shows a calculated footstep sequence for a model of the humanoid robot
H7 navigating in the laboratory. The sequence is computed to reach a circulargoal region determined by a red ball that is tracked in real time. Figure 14 shows
snapshots taken while running the goal following planning program. There were atotal of 19 discrete foot placements considered for each foot in the online versionof the planner.

5

Discussion

Due to the computation time involved in planning some of the longer paths, atiered planned can be used which ﬁrst simply plans for a robot-sized circle tomove through to environment. If a path is found, footsteps can be ﬁlled in along
this path using heuristics. This allows the planner to leverage the substantial
research for mobile robot planning, and fall back to this more detailed planning
whenever the ﬁrst round of fast planning fails.
Careful thought needs to go into designing transition sets. Lower numbersof transitions result in a lower branching factor and faster run times, sometimes
much faster, but can result in slower times and sometimes no solution when theset of transitions does not suﬃciently cover the space of possible transitions. Also,
it is not just the number of transitions that is important, but their arrangement
and associated costs.
Best-ﬁrst search consistently outperforms A* search except in cases of severe
local minima, but can yield undesirable paths in the way it circumvents obstacles

in the path. By increasing the weight of the estimated cost heuristic relative
to the location cost and step cost, A* can be brought closer to best-ﬁrst by
sacriﬁcing some optimality to lower the running time, depending on the user’s
requirements for both optimality and speed.
Many improvements could be built upon this work to increase the capabilitiesof the algorithm, including:
1. State- or Transition- dependent transitions, allowing for diﬀerent gaits or
actions to be encoded.
2. Calculation of Waypoints, to break a large planning problem into shorter
problems to avoid the exponential growth that comes with more obstacles.
3. Dynamically generated transitions which can increase or decrease the number of transitions depending on the environment, or adjust the locations oftransitions to better match the terrain (e.g. set stride length to match stairlength).
We are currently working on extending to algorithm in these directions, as wellas conducting more complex, vision-guided footstep planning experiments with
the H7 humanoid robot.

References[Gir87]

M. Girard. Interactive design of computer-animated legged animal motion.
IEEE Computer Graphics & Applications, 7(6):39–51, June 1987.
[HHHT98] K. Hirai, M. Hirose, Y. Haikawa, and T. Takenaka. The developmentof honda humanoid robot. In Proc. IEEE Int’l Conf. on Robotics andAutomation (ICRA’98), pages 1321–1326, May 1998.
[Hir84]
S. Hirose. A study of design and control of a quadruped walking vehicle.
Int. J. Robotics Research., 3(2):113–133, Summer 1984.
[KMB95] E. Kokkevis, D. Metaxas, and N. I. Badler. Autonomous animation andcontrol of four-legged animals. In Proc. Graphics Interface, pages 10–17,
May 1995. ISBN 0-9695338-4-5.
[kNa]
k. Nishiwaki and. In Proc. IEEE/RSJ Int. Conf. on Intelligent Robots andSystems (IROS’02).
J. Kuﬀner, K. Nishiwaki, S. Kagami, M. Inaba, and H. Inoue. In Proc.
[KNK+ ]
IEEE/RSJ Int. Conf. on Intelligent Robots and Systems (IROS’01).
[LDS+ 00] O. Lorch, J. Denk, J. F. Seara, M. Buss, F. Freyberger, and G. Schmidt.
ViGWaM - an emulation environment for a vision guided virtual walking
machine. In Proc. IEEE Int. Conf. on Humanoid Robotics (Humanoids
2000), 2000.
[NII99]K. Nagasaka, M. Inaba, and H. Inoue. Walking pattern generation for ahumanoid robot based on optimal gradient method. In Proc. IEEE Int.
Conf. on Systems, Man, and Cybernetics, 1999.
[NSK+ 01] K. Nishiwaki, T. Sugihara, S. KAGAMI, M. Inaba, and H. Inoue. Online
mixture and connection of basic motions for humanoid walking control
by footprint speciﬁcation. In Proc. IEEE Int’l Conf. on Robotics andAutomation (ICRA’01), Seoul, Korea, May 2001.

[PAM+ 96] A.E. Patla, A. Adkin, C. Martin, R. Holden, and S. Prentice. Characteristics of voluntary visual sampling of the environment for safe locomotion
over diﬀerent terrains. Exp. Brain Res., 112:513–522, 1996.
[Pat98]
A.E. Patla. How is human gait controlled by vision? Ecological Psychology,
10:287–302, 1998.
[PNS00]A.E. Patla, E. Niechwiej, and L. Santos. Local path planning during humanlocomotion over irregular terrain. 2000.
[PP99]
J. Pratt and G. Pratt. Exploiting natural dynamics in the control of a
3d bipedal walking simulation. In In Proc. of Int. Conf. on Climbing andWalking Robots (CLAWAR99), September 1999.
[TvdP98] N. Torkos and M. van de Panne. Footprint-based quadruped motion synthesis. In Proc. Graphics Interface, pages 151–160, 1998.
[VBSS90] M. Vukobratovic, B. Borovac, D. Surla, and D. Stokic. Biped Locomotion:
Dynamics, Stability, Control, and Applications. Springer-Verlag, Berlin,
1990.
[vdP97]
M. van de Panne. From footprints to animation. In Proc. Computer
Graphics Forum, volume 16, pages 211–223, October 1997.
[YINT98] J. Yamaguchi, S. Inoue, D. Nishino, and A. Takanishi. Development of
a bipedal humanoid robot having antagonistic driven joints and three dof
trunk. In Proc. IEEE/RSJ Int. Conf. on Intelligent Robots and Systems(IROS’98), pages 96–101, 1998.
[YL99]
M. Yagi and V. Lumelsky. Biped robot locomotion in scenes with unknown obstacles. In Proc. IEEE Int’l Conf. on Robotics and Automation(ICRA’99), pages 375–380, Detroit, MI, May 1999.