Real-Time Path Planning for Humanoid Robot Navigation

1 Introduction

Path-planning is one of the fundamental problems in mobile robot navigation. It has been shown long before that the problem of moving an object through space is PSPACE-hard with a time complexity exponential in the degrees of freedom of the object [Latombe, 1991]. In mobile robots the degrees of freedom are usually small (three or less) which opens the application of a range of search techniques speci�cally tailored to the problem. If one can neglect orientation, e.g. a cylindrical holonomic system, the resulting 2 dimensional con�guration space can be searched ef�ciently by A*, D* [Stentz, 1995], or dynamic programming [Buhmann et al., 1995].

The higher degrees of freedom can be tackled e.g. by probabilistic roadmaps [Kavraki et al., 1996]. However, if one can employ a non-probabilistic approach by �nding a suitable approximation then the solution can be seen preferable due to its deterministic nature. One possible approximation is to model the robot as a cylinder or by its convex hull which allows for ef�cient collision checking [Okada et al., 2003].

Humanoid robots while being mobile allow for more than the usual three degrees of freedom. Their feet can be placed with a greater choice and the change of body posture allows to overcome obstacles where wheeled robots fail in passing through. This enables humanoids to step onto objects [Kuffner et al., 2002], climb up and down stairs [Hirai et al., 1998; Gutmann et al., 2004], step over obstacles [Guan et al., 2004], or crawl underneath objects [Shiller et al., 2001; Kanehiro et al., 2004].

Care should be taken though not to over-simplify the problem. Humanoid robots are non-holonomic, i.e. they cannot turn in place without requiring additional space. The trajectory of the body center describes a curve similar to a car-like robot, although the turn radius is generally quite small. It is possible to account for the extra turning space in a cylinder model of the robot enlarged by twice the turn radius [Li et al., 2003; Sabe et al., 2004]. However such an approximation is overly pessimistic and prevents the robot from passing through narrow space as we will show.

Discretizing the possible actions of the robot into a small set of well-chosen actions, e.g. different foot placements, leads to another way for �nding solutions ef�ciently. The induced search graph grows exponentially with the number of steps and thus, only a small number of foot steps can be explored in a real-time system [Lorch et al., 2002]. Such a foot-step planning system is nevertheless a valuable tool in a hybrid system where a higher level planner provides waypoints for this sub-system [Chestnutt and Kuffner, 2004].

Shiller et al. [2001] discretize the actions and orientations of a digital �gure that can walk forward, sideways, turn and crawl. They maintain an obstacle space for each combination of action and orientation and �nd solutions ef�ciently for trajectories of several ten steps.

We compute a trajectory for the body-center of a humanoid robot by approximating the shape of the robot using multiple cylinders and discretizing con�gurations into a set of orientations. When following this trajectory, the next few foot steps are computed using an analytical method. This enables a humanoid robot to �nd paths of up to a few meters in real-time and includes actions such as sideways walking through narrow space and climbing up and down stairs.

Our work differs from Shiller's in that we do not plan behaviors but select the behavior depending on the environment when following a found path, e.g. stair-climbing over a sill. We also maintain only one obstacle space and show how to perform collision checking ef�ciently. Last but not least, our system is demonstrated on a real robot where stereo range data incrementally updates a representation of the world.

Our approximations of con�guration space and actions are presented in the next section. Section 3 brie�y describes our approach for generating a navigation map for collision checking. Section 4 details our application of the A* algorithm for path searching. Experimental results on Sony's humanoid robot QRIO are described in Section 5. Finally, we discuss limitations and extensions of our approach in Section 6.

2 Con�guration Space and Actions

The robot moves in a 2 dimensional world. Obstacles, different �oor levels and stairs are projected onto one plane. We do not neglect the orientation of the non-holonomic robot, thus the con�gurations space C is 3 dimensional. For allowing ef�cient search we discretize C into a regular grid of evenly spaced cells using 8 different orientations:

We restrict the movement of the robot's body center to the following six actions A = { forward, backward, turn-left, turn-right, side-left, side-right}. For each action the transition in C is given for orientations 0◦ and 45◦ in Figure 1. For other orientations the transitions are analogous.

where cs de�nes the cell size and n the number of cells.

When concatenating turn-left (or turn-right) actions, the resulting trajectory describes a curve, see Figure 2(a). The turn actions thus model the non-holonomic constraint of a humanoid where changing the orientation requires extra space for turning. If we approximate this curve with an arc of a circle, we �nd that the cell size cs coincides with the turn radius of the robot and therefore cs should be chosen as such.

The cylinders are a tight �t to the shape of the robot standing still. When the robot moves, however, additional space is needed for the transition in C as body and legs are swinging while walking. Figure 3 shows snapshots of the robot when moving forward, turning left and stepping sideways. We account for the additional space by enlarging the cylinders at the start and end con�gurations depending on the action. Both, upper and lower, cylinders are enlarged by the same amount. Only when walking sideways, no additional space is needed. As we will see in Section 5 this enables the robot to pass through narrow space by choosing the sideways walk.

The shape of the robot is approximated by two cylinders sharing the same axis as shown in Figure 2(b). Cylinders allow for fast collision checking and the smaller cylinder for the legs enables the robot to pass close to low obstacles where upper body and arms are above the obstacle.

It is worth noting that the humanoid robot does not necessarily need to be able to exactly perform the 6 actions de�ned in this section. For example, the robot could well use several foot steps for performing the 45◦ rotation in our turn actions. The only requirement is that the enlarged cylinders cover all the space the robot requires for completing the 45◦ turn.

Figure 2: (a) Turn around by concatenating 4 turn-left actions. (b) Multi-cylinder model for approximating robot shape.

One might think of our approach to be speci�cally tailored to humanoids as the proposed actions and the cell size matching the turn radius are probably only found for this kind of robot. While this seems to be true, the contribution of this work is to demonstrate the abilities of such a system in a complex real-world environment. We will discuss the limitations and possible extensions of our approach in Section 6.

3 NavigationMap and Collision Checking

Collision checking is one of the crucial factors in path planning. Cylinder models of robots allow to perform this check in constant time if the distance to the nearest obstacle is known; the distance is simply compared to the radius of the cylinder. Our two cylinder model allows to check for collision in the same amount of time.

For ef�cient collision checking we make the following assumptions about the world the robot is navigating in:

1. The world is partitioned into �oor and obstacles.
2. Floor is planar and horizontal (else it is an obstacle).
3. There are no multiple �oor levels at the same location.
4. The robot is able to distinguish between �oor and obstacles and can can estimate their relative position and height using its sensors.

Assumption 1 is general, 2 rules out inclined surfaces and treats them like obstacles, 3 is true for most but a few environments and enables the use of a 2.5D map for representing the world. Finally, assumption 4 is a practical consideration and can be achieved e.g. by plane extraction from range data. 1

Under these assumptions, the robot is able to build a �oor and obstacle height map FOG of its environment. We employ a combination of a 3D occupancy grid with a 2.5D �oor height map where range data from stereo vision is segmented into planes and integrated in a probabilistic way [Gutmann et al., 2005]. Each cell in FOG holds a type and a height:

From this map a navigation grid NAV is computed storing re�ned �oor types and distances to nearest obstacles:

with x ∈ X, y ∈ Y, t ∈ T = {floor, obstacle, unknown}, and h ∈ R is the height of the �oor or obstacle.

where t′ ∈ T ′ = T ∪ {stairs, border} and d ∈ R is the clearance, i.e. the distance to the closest obstacle for t′ ∈ {f loor, stairs}. NAV re�nes the type of FOG(x, y) = (t, h) for t = floor in the following way. Let U(x, y) be a neighborhood around position (x, y) and ∆ˆh be the maximum absolute difference in �oor height to cells in U(x, y):

The re�ned type is then computed as:

The re�nement of �oor cells allows to treat stairs and borders differently from regular �oor. We will make use of stairs in path-planning by assigning a different cost for traversal. Borders are treated similar to obstacles and as such prevent the robot from getting close to or over the end of a �oor layer.

where df loor is the maximum step height for regular walk and dstairs the corresponding one for climbing up or down stairs.

The clearance of a cell is de�ned in the following way. For a cell (x, y) with FOG(x, y) = (floor, h) we compute the difference in height ∆h(x′ , y′ ) to cell (x′ , y′ ) with FOG(x′ , y′ ) = (t′ , h′ ), refine(t′ ) ∈ {obstacle, border}:

This height difference then decides which cylinder of our two-cylinder model comes into effect for computing the distance d′ (x′ , y′ ) to this obstacle, or if it can be ignored:

where e = √(x′ - x)2 + (y′ - y)2 is the Euclidean distance between the cells, and rl, ru and hl, hu are the radii and heights of the two cylinders as depicted in Figure 2 (b).

1 It should be possible to relax these assumptions and allow for inclined surfaces. However, the model for representing the world would become more complicated which is why we do not consider them in this paper.

The clearance d(floor) of cell (x, y) is then de�ned as the minimum distance to all obstacles and borders.

The computation of this value can be implemented ef�ciently by region growing of obstacles and borders until a preset maximum distance is reached.

Note that (8) only applies for cells where NAV (x, y) = (floor, ·). For other types t ∈ T the clearances are set to:

For the robot being able to stand at a given position (x, y) with NAV(x, y) = (·, d), the clearance d must be positive in order not to collide with an obstacle. For unknown terrain we are optimistic in that there could be a �oor height well above all obstacles and thus ignore obstacles.

We can de�ne the free con�guration space Cfree as

As an example Figure 4(a) shows a simulated world fully observed by an ideal sensor. The resulting NAV grid is shown in Figure 4(b). Floor heights are drawn in white, yellow and green, stairs are marked with brown crosses, the border is �lled in brown, and obstacles are black. Different shades of gray show the clearance of cells where brighter cells indicate a larger clearance than darker ones. Note how the taller obstacle in the bottom right grows more to the bottom and right of the map (where the distance to the �oor is larger than hl) and less to the higher level �oor.

4 Path-Planning

We have now the basic tools for de�ning the path-planning problem in our domain. Let c, c′ ∈ C be con�gurations. The transition c′ = succ(c, a) for a single action a ∈ A is given in Figure 1. We de�ne succ for a sequence of actions a1a2 . . . am in a recursive way as

In order to test whether action a is applicable in con�guration c, we check the type and clearance of c in NAV:

where allow(c, a) forbids certain actions depending on the type t′ in NAV at the position of c (e.g. we do not allow backward and sideways walk in unknown terrain and only allow forward walk on stairs), ra is additional space required for execution of a (see Figure 3), and

We also de�ne app on a sequence of actions a1a2 . . . am

and call the sequence a path if (14) holds.

The problem of path-planning for given start and goal con�gurations cstart, cgoal ∈ Cfree is then to �nd a path π = a1a2 . . . am such that

Problems for which a path exists are called solvable. In general there can be more than one solution for a given solvable problem. We are interested in an optimal path, i.e. a path ˆπ that minimizes a cost function g over the path:

In our system we found the following cost function valuable:

where c′ = succ(c, a1), ga is a cost depending on the action (e.g. forward walk has less cost than sideways which takes more time), gt′ a cost depending on the type t′ in the NAV grid (floor is preferred over stairs and unknown), gc a constant for changes in action (the robot might have to do additional foot steps), and go considers the clearance of c′ in NAV in order to prefer safer paths more distant from obstacles:

We employ the well-known A* algorithm for �nding solutions. A* maintains a priority queue where candidates are ranked according to an evaluation function f composed of the cost function g and an estimated heuristic h to cgoal:

Here dmax is the maximum clearance above which no further improvement in safety is expected, d is de�ned as in (13), and ˆ go is a prede�ned constant.

For our chosen discretization of C we can employ the following heuristic which is optimistic and a larger cost estimate than the Euclidean distance:

where ∆x = |x-xg | and ∆y = |y -yg| are absolute coordinate differences from c = (x, y, ·) to cgoal = (xg, yg, ·) and ˆ ga is the minimum action cost.

An important factor for the ef�ciency of A* is to detect whether a con�guration has already been visited before. By using a regular grid, we can pre-compute the search graph (but not the optimal paths since the environment is unknown) and check for duplicates in constant time. This is an advantage over methods that build the tree at the time of search as checking for duplicates needs further processing e.g. by applying nearest-neighbor methods.

From the properties of the A* algorithm and our chosen cost and heuristic functions it follows that A* �nds the cost optimal path ˆ π according to (16) if a solution exists.

5 Results

We implemented our approach on QRIO, Sony's small humanoid robot with 38 degrees of freedom [Fujita et al., 2003]. The robot is equipped with 3 MIPS R5000 CPUs clocked at 400MHz and a stereo camera system with a 45◦ �eld of view providing disparity images at 12.5 fps. This stereo data is segmented into planes from which our FOG and NAV grid representations are updated in real-time.

Table 1 lists parameters used in our implementation for all variables introduced in the previous sections. For stairs we restrict actions to forward walk only. Thus, the robot only �nds paths on straight staircases unless there is a larger �oor area where turns are possible. We forbid sideways and backward walk in unknown terrain because the robot is not able to see into its moving direction. We set a high cost for backward motion since it is generally more unsafe. Costs are added for walking in unknown terrain and on stairs which focuses the algorithm to search for a path on known �oor even if the distance traveled is larger than when moving on other terrain.

For our experiments we built a 4 meters long and 1 meter wide stage where several obstacles, a 4cm high sill, a staircase with 4 steps each 3cm high, and a higher-level platform were arranged as shown in Figure 5.

We placed QRIO at the left end of the stage and let it observe the environment by rotating on the spot. We then commanded the robot to �nd and follow a path to a position 3 meters ahead (the center of the higher-level platform). Our path-planning system continuously evaluates the current NAV

grid and sends paths to a path-following system which computes the next two foot steps and feeds them to the robot's motion control system. Figures 6�8 show 3 snapshots of the robot while moving on the stage. We use the same encoding for �oor, border, stairs and obstacles as before. The color indicator on the right shows the �oor height as estimated by the robot. Unknown space is left blank. Green dots mark the search space of the A* algorithmwhile the path is shown with blue arrows. Brown bars on the path indicate stair transitions.

After passing through the narrow passage, the robot plans a path over the sill in the center of the stage (see Figure 7). For overcoming the sill, the system employs a separately developed stair-climbing module [Gutmann et al., 2004].

In the beginning the system has to �nd a path leading between two close obstacles (see Figure 6). Since space between the obstacles is narrow, forward walk is not possible due to the lack of the required clearance rforward. Therefore, the system decides to walk sideways between the obstacles.

Finally, as the robot walks closer to the higher platform, it �nds a path on the staircase to the target location (Figure 8).

The worst case run-time occurs when the goal is placed at an unreachable location, e.g. a free position completely surrounded by obstacles. In this case all but a few of the 8n2 = 80,000 con�gurations have to be examined. The robot's embedded processor needs close to 900 ms for exploring this search space (110 ms on a Pentium III, 1.4GHz).

The time required for �nding a path on our �xed-size con�guration space depends on the environment and the path length. In the examples above the time for planning was always below 100 ms on the robot's embedded CPU. In order to investigate the run-time dependency on path length we randomly selected goal con�gurations in the �nal map after the robot reached its target. If a path to the goal could be found, we recorded the length of the path and the time needed for computation. Figure 9 shows the runtime versus path length measured on a Pentium III, 1.4GHz computer. Although the curve shows a slope at least quadratic in path length, for our chosen dimensions of C the runtime increases only slowly and does not exceed 40 ms even for longer paths.

6 Conclusion

We presented and evaluated a path-planning algorithm that provides collision free trajectories of up to a few meters for a humanoid robot in a layered environment in real time.

In our approach unknown terrain is treated in an optimistic way, i.e. their clearing is assumed arbitrary large. As the robot moves closer to unknown area and observes the situation, the navigation map is re�ned and a new path is planned automatically. This allows navigation in unknown environments.

The limitations of our approach are various. Our discretization onto a grid is an approximate cell decomposition and as such is only complete up to the resolution of the grid. Furthermore, using only 8 orientations can prevent �nding paths in certain environments, for example in a narrow corridor oriented at an angle of 20◦.

Our perception system distinguishes between �oor, border, stairs and obstacles and interprets them in a meaningful way for path planning. We believe our approach advances previous methods on simulated worlds, and real robot systems with mainly reactive, short distance navigation capabilities.

Our approach may be extended to 2m orientations (m > 3) by changing cell size and actions such that repeated application of a turn action describes a curve with the minimum turn radius of the robot. Other actions might move the robot over more than one cell in order to align with the grid.

At current, only straight paths are allowed on stairs. By introducing new actions left-forward and right-forward we can also �nd paths on spiral staircases at the cost of a higher runtime. In practice, we observed an increase of about 25% in run-time for the worst case scenario described in Section 5 when adding these two actions.

Our set of actions and the navigation map do not consider the possibility of stepping over an obstacle. We believe that for such a capability, a foot step planner using precise 3D information is of importance. However, our approach could be combined with such a system by marking obstacles the robot can step over similar to as we mark stairs.

We think it is also possible to extend our approach to �nd areas where the robot can crawl underneath an obstacle. Our current �oor and obstacle map and the navigation grid, however, do not support such a detection yet and have to be extended �rst. We will follow this direction in the future.

Acknowledgment

We would like to thank Stefan Edelkamp, Thilo Weigel and the anonymous reviewers for their valuable comments. We also thank all developers of the Sony QRIO robot for making this work possible.