Journal of Intelligent and Robotic Systems 33: 25–44, 2002. © 2002 Kluwer Academic Publishers. Printed in the Netherlands.

25

Speed Planning for a Maneuvering Motion KAO-SHING HWANG and MING-YI JU Department of Electrical Engineering, National Chung Cheng University, 621 Chia-Yi, Taiwan; e-mail: [email protected] (Received: 12 January 2000; in final form: 18 April 2001) Abstract. Collision-free motion among moving objects is an on-going research topic. Based on the concept of a modified path-velocity decomposition and application of the interface propagation method, a strategy for trajectory planning is proposed in this paper. In the proposed method, the global navigation paths for robots are assumed to have already been planned without any static obstacle crossing their paths. Each subtask along the global path of each controlled object contains a desired goal position and desired arrival time for reaching the position. Based on the information about each subtask, Space/Time Graphs (STGs) for the robots are created. By shifting the speed path from corresponding forbidden regions on the STG, potential collisions can be avoided. Optimal speed paths with least velocity alterations for controllable objects are derived automatically by applying the interface propagation method in the STGs. The applicability of the proposed approach is demonstrated and the results show that controllable and uncontrollable moving objects can work together in a shared environment by avoiding collisions. Key words: trajectory planning, speed alteration, space/time graph, maneuvering motion, interface propagation method.

1. Introduction Motion planning is an important topic in the applications of robotics. The research efforts aim at determining efficient methods for collision avoidance among static and dynamic obstacles in a shared environment. Most previous research into the motion planning problem were concentrated only on the subproblem of finding collision-free paths among static obstacles (Udupa, 1977; Lozano-Pérez, 1979, 1981). Geometrically, it may be viewed in two complementary ways: planning a path so that it avoids obstacles or planning a path so that it lies in a free space, i.e., physical space minus obstacles. The path planning among static obstacles is the initial step in controlling an object to fix a navigation path. In practice, it is assumed that this route, i.e., global navigation, has been chosen basing upon the criterion and locations of static obstacles. The criterion chosen may be a minimal distance path or another meaningful distance measure. However, those methods do not generalize since there is no notion of the “time” parameter in their formulations. Once a path has been found, the robot’s velocity along it does not matter. Such static problems are what we refer to as path planning problems. Therefore, in this paper, we will concentrate on the planning among dynamic objects. In other words,

26

K.-S. HWANG AND M.-Y. JU

we will assume that there are no static obstacles along the desired path of any dynamic moving object. For coordinating moving objects in a shared environment, an approach based on the construction of a task-completion diagram was proposed for collision-free trajectory planning (O’Donnell et al., 1989). However, it can be applied only to the case of two controllable objects moving in a shared workspace. Besides, additional SW-closure operations are requested to prevent the deadlock condition. Based on Configuration-Space-Time, a method for path coordination among multiple robots was proposed (Warren, 1990). This method first assigns to each robot a priority. A path that avoids only the stationary obstacle is planned for the highest priority robot. Then a trajectory for the next lower priority robot is planned so that it avoids both the stationary obstacles and a higher priority robot which is treated as a moving obstacle. Nevertheless, the construction of Configuration-Space-Time for path coordination is time-consuming. In the presence of moving obstacles, many different velocity functions on the same path may lead to a collision. The problem of planning the velocity for a robot moving along a specific path to avoid collision with moving obstacles should be taken into account. The proposed approach to the velocity planning exploits a natural decoupling of the problem into two components: the spatial distance and traveling time. In a space/time graph (STG), the spontaneous speed can be determined by the slope of the line segment bypass of the regions which represent where and when a collision would occur if the robot moves at the original speed. The velocity planning problem is transformed into a path planning problem so that the trajectory planning is reduced to a two-fold path planning problem and rich methods proposed for the path planning can be applied. For instance, the tangent graph method (Liu, 1994) or the visibility graph method (Kant, 1986) can be applied to an STG in resolving a velocity planning problem (Fraichard, 1993; Fraichard, 1994). However, feasible paths generated by the tangent graph methods are a set of line segment speed paths, which are discontinuous in view of the velocity profile. This implies that the object is commanded by an infinite acceleration/deceleration at each adjacent point. This is unrealizable in a physical system. Furthermore, the above methods also suffer from computational complexity as the number of obstacles increases. Therefore, an alternative method to overcome those drawbacks is needed. Interface propagation methods have been widely used in image process applications in recent years (Malladi et al., 1995; Malladi, 1996; Debreuve, 1998). The underlying principle is the evolution of a simple closed curve whose points move in the normal direction at a prescribed velocity. In a sense, the evolution of an interface can be viewed as a searching procedure according to some criteria. In this paper, a systematic approach for generating a smooth velocity profile for a maneuvering motion is proposed. The central idea is to construct the STGs of the controllable objects and then apply the interface propagating method in the STGs to generate collision-free trajectories.

SPEED PLANNING FOR A MANEUVERING MOTION

27

This article is arranged in the following way. In Section 2, the technology of how to construct a Space/Time Graph (STG) is introduced. Based on the STG, the generation of a velocity profile is described in Section 3. The use of the propsed method for velocity profile generation is presented in Section 4. In Section 5, simulation results are reported in which the proposed method, which consists of the construction of STGs and the use of the interface propagation method, is applied to scheduling the smooth velocity profile under the constraints of the maximal speed allowed and the maximal acceleration/deceleration for collision-free maneuvering motions of multiple controllable objects in a dynamic environment. Finally, a conclusion is given in the last section. 2. Space/Time Graph A space/time graph (STG) is a 2-dimensional figure with travelling time versus travelling distance. A moving controllable object, e.g., a mobile robot, to be planned is treated as a point in a STG. If the object is moving at a constant speed, both travelling time and travelling distance units within this graph are normalized in a way that a speed path for the object is a straight line 45 degrees from the distance axis (i.e., the slope of this line is 1), beginning at the origin of the graph as in Figure 1. Therefore, the units of this graph are expressed by a scaled Distance Unit (DU) and a corresponding Time Unit (TU) of the object to be planned. As for the goal of the motion, it is represented by a vertical line (parallel to the time axis, i.e., Y -axis) at the desired final position on the distance axis of a STG. Therefore, if the object speed is adjusted, it is very easy to determine the time discrepancy for reaching the desired position. To avoid a possible collision between two objects, i and j , there are forbidden regions along the path for each object that may incur the collision with the other object. Each region consists of two parameters: Maximum Forbidden Distance (MFD) and Forbidden Time Interval (FTI). MFD(i, j ) represents a possible contact

Figure 1. Space/time graph properties.

28

K.-S. HWANG AND M.-Y. JU

Figure 2. Collision region graph of two straight-line paths.

distance of object i, with object j measured along the path of object i. FTI(i, j ) represents the maximum possible contact time for object j passing through the corresponding forbidden distance MFD(i, j ) on the path of object j . Vj represents the speed of object j , i.e., FTI(i, j ) =

MFD(j, i) . Vj

(1)

For generality and simplicity, the bounds of the forbidden regions are approximated by setting a safety distance R for each dynamic object. This distance forms a bound along the motion of the object, representing the margin for avoiding collisions. Based upon this concept of safety distance, the MFD of a multi-body object, such as a robot, can be estimated through simulation. To illustrate this concept using two colliding single-body objects, the forbidden region parameters are displayed in Figure 2. The MFDs can be easily estimated from the geometry of collisions as follows: MFD(i, j ) = 2Ri cotα + 2Rj cscα, MFD(j, i) = 2Rj cotα + 2Ri cscα,

(2) (3)

where the collision angle α will be replaced by 180◦ − α if the collision angle is more than 90◦ . Through estimating the positions of other moving objects at various times, all the information, i.e., the defined forbidden time and distance, about the possible collisions within the environment can be mapped into the STG of the planning object, which is called a dominant object. For example, Figure 2 shows a collision between objects i and j . The collision condition of object j due to the motion of object i can be estimated by MFD(i, j ) and FTI(i, j ) as in Equations (1)–(3). These two values can be easily represented as a rectangle, i.e., the forbidden region, in the space/time graph shown in Figure 1. Based on the space/time graph, a feasible collision-free motion of the dominant controllable object can be obtained by avoiding the forbidden regions formed by the other moving objects. Therefore, the

SPEED PLANNING FOR A MANEUVERING MOTION

29

collision avoidance problem in a dynamic environment can be simplified to plan a collision-free speed path among the static forbidden regions.

3. Velocity Profile Generation The velocity alteration process is manipulated as follows. In the beginning, all subtasks are identified by a desired global navigation path for each controllable or uncontrollable object. Since the problem with static obstacles is not taken into consideration in this paper, it assumes that all global navigation paths have already been planned without any static obstacle in their paths. Each subtask along the global path of each object contains a desired goal position and desired arrival time for reaching the position. Based on the information gathered about each subtask, STGs are created for the controllable objects. Eventually, several appropriate speed paths for the controllable object can be found from the corresponding STG. In order to create safe motions for all moving objects simultaneously, the dominant object should be controlled to shift its speed path on the STG from associative corresponding forbidden regions to avoid potential collisions. Finally, the planning information of STGs for the next subtask will be updated based on the results obtained from previous procedures. By repeatedly executing these procedures until all subtasks along global paths are finished, the optimal motion planning for all dynamic objects can be obtained.

3.1. SPACE / TIME GRAPH GENERATION For each subtask along the global paths of controllable objects, the desired goal position and arrival time are obtained at a desired velocity. Initially, only uncontrollable objects in the environment are taken into account. A priority was assigned to each controllable moving object and a lower priority object treated the higher priority objects as moving obstacles when the planning speed path. By planning the speed paths on the STGs of the controllable moving objects one at a time, the dimensionality of the problem is reduced. The controllable moving object of highest priority will be chosen as a dominant object. Based on the current motions of moving obstacles, all possible collisions along the planned path of the dominant object are located. The parameters of the forbidden regions are then calculated and mapped onto the corresponding STG of the dominant object. According to the desired velocity and the goal point of this task, a constant speed path and a goal line for a STG can be created. The same procedure for constructing a STG is applied to the controllable objects of lower priorities as the collision-free velocity profiles of the ones with higher priorities have been generated.

30

K.-S. HWANG AND M.-Y. JU

Figure 3. Constraints on the speed path in STG.

3.2. CONSTRAINTS OF VELOCITY PROFILE GENERATION In real applications, the velocity of any controllable moving object will have a limit range due to the physical constraint such as the maximum torque of actuators. Therefore, the velocity of a moving object should not exceed the maximal speed allowed. Besides, according to the maximal acceleration/deceleration of a controllable moving object, the STG should be modified by adding a velocity-zone cone with the upper speed limit (Vmax ) and lower speed (Vmin ) limits as edges of the cone, as shown in Figure 3. To avoid forbidden regions, the range of the cone will be used to determine adequate speeds for the dominant object and the subordinates. The derived speed should not violate the constraints. The slope of the derived speed path in an STG will be less than 1 if it is faster than its initial speed. On the other hand, the speed is slower than the pre-assigned one as the slope of the speed path is larger than 1.0. Based on the information represented in an STG, the motion of the controllable object can be adjusted to avoid possible collisions. Since there are many possible adjustments from speed changes, a planning algorithm will be needed to select the optimal one. 3.3. CONTINUITY OF VELOCITY PROFILE Based on an STG, many possible collision-free speed paths can be found by avoiding the forbidden regions. By these paths, the object can reach the goal position without any collision. Due to the modification of speed, the goal position may not be reached at the desired arrival time. There are many approaches for generating feasible speed paths. The easiest way, such as the visibility graph method, is to generate a number of speed paths for the dominant object within the STG, as shown in Figure 4. As a matter of fact, the feasible speed paths selected from Figure 4 are a set of line segments, which is discontinuous in view of the velocity profile, as shown

SPEED PLANNING FOR A MANEUVERING MOTION

31

Figure 4. Aggregation of line piecewise velocity profile (Hwang, 1999).

Figure 5. Velocity profile of the controllable moving obstacle generated by using a visible graph.

in Figure 5. In other words, these speed paths just hold C 0 continuity. This implies that the object is commanded by an infinite acceleration or deceleration at each adjacent point. This is unrealizable for a physical system. To induce a more realistic resolution, the recommended speed path requires at least C 1 continuity. This criterion may be satisfied by smoothly interpolating between the vertices with quadric splines (De Boor, 1978). However, care must be taken to guarantee that the interpolated function does not intersect the forbidden constraint regions. This involves some heuristic strategies. Manual tuning is usually required. Alternatively, a more systematic method based on interface propagating modeling is proposed to solve this problem.

32

K.-S. HWANG AND M.-Y. JU

4. Interface Propagation Method The underlying concept of the interface propagation method is the evolution of a simple closed curve whose points move in the normal direction at a prescribed velocity. Take a water wave as an example; a rock tossed into water creates a circular disturbance which travels outwards in all directions. Waves travelling from the shallow end to the deep end can be seen to refract, i.e., bend, increase in wavelength (the wave fronts become looser) and speed up (they take less time to travel the same distance). Besides refraction, reflection, and diffraction are also well-known behaviors of water waves. Reflection involves a change in the direction of waves when they bounce off a barrier; diffraction also involves a change in the direction of waves as they pass through an opening or around a barrier in their path. Water waves have the ability to travel around corners and obstacles, and through openings. An example is shown in Figure 6. This ability is most obvious for water waves with longer wavelengths. The fast marching method is one of the numerical techniques used to track the evolution of a wave front. By using the level set techniques to numerically approximate the evolution of a propagating interface, the behavior of an interface can be described by a partial differential equation whose unique solution gives the position of the front. More precisely, the level set techniques view a moving interface as the zero level set of a function φ(x, t = 0). Therefore, an evolution equation for the interface moving with speed F in its normal direction is given by φt + F |∇φ| = 0.

(4)

Assume the propagating speed F to be either always positive or always negative, therefore the level set formulation can be converted from a time-dependent partial differential equation into a stationary one in which the time parameter disappeared. Instead of the time parameter, a time-independent stationary level set formulation can be constructed to translate the level set φ into a function T (x, y), i.e., the travel time at which the front propagates across the point (x, y). Then it satisfies the

Figure 6. (a) Water waves. (b) Waves travel from the shallow end to the deep end. (c) Two objects are located in the space through which waves travel (the reflection behavior is not taken into consideration).

33

SPEED PLANNING FOR A MANEUVERING MOTION

following equation: |∇T |F = 1.

(5)

If the propagating speed F is a function of position only, then, in fact, Equation (5) is an Eikonal equation (Van Trier and Symes, 1991; Qin et al., 1992), namely, |∇T |F (x, y) = 1.

(6)

The above equation indicates that the gradient of arrival time is inversely proportional to the propagating speed of the front. If the speed function is always positive, then the crossing surface T (x, y) is single-valued. By properly defining the propagating speed in the workspace, the evolution of an interface is able to search for a minimal-time and collision-free path from the starting point to the goal position. The key in level set methods is to approximate the gradient term in Equation (6) in a way that satisfies the correct entropy condition (Sethian, 1985; Leymarie and Levine, 1992). To solve the above equation, an approximation which used finite difference operators on a fixed Cartesian grid is needed. An upwind approximation to the gradient leads to
 −x 2  +x 2  −y 2  +y 2 max Di,j T , 0 + min Di,j T , 0 + max Di,j T , 0 + min Di,j T , 0 =

1 , F (i, j )

(7)

where T (i, j ) − T (i − 1, j ) , x T (i, j ) − T (i, j − 1) −y , Di,j T = y −x T = Di,j

T (i + 1, j ) − T (i, j ) , x T (i, j + 1) − T (i, j ) +y Di,j T = ; y

+x Di,j T =

and

T (i, j ) is the travelling time to the position (i, j ) of the grid point, x is the distance between two grid points on the x axis, and y is the distance between two grid points on the y axis. According to the upwind difference structure of Equation (7), the interface propagates in “one way” from smaller values of travel time T to larger ones. The specific causality relationship is utilized for developing a systematic method to produce the solution. 4.1. FAST MARCHING METHOD Solving for the travel time T given in Equation (7) efficiently is the most important problem for the propagating interface model. Since Equation (7) is a quadratic equation, one can iterate until convergence is obtained by solving the equation at each grid point. However, this scheme is very time-consuming. Therefore, a more efficient method called the fast marching method is applied to solve this quadratic equation. This method is based on the observation that the upwind difference struc-

34

K.-S. HWANG AND M.-Y. JU

ture of Equation (7) means that information propagates from small values of T to larger values. Hence, the algorithm is based on the idea of building a solution of Equation (7) outwards from the smallest T value. Motivated by the methods given in (Chopp, 1993), the “building zone” is confined to a narrow band around the front. The idea is to sweep the front ahead in an upwind manner by considering a set of points in the narrow band around the current front, and then to march this narrow band forward. The fast marching method is divided into two stages: the initialization stage and the marching forward stage. In the initialization stage, the initial front starts from a grid point with T = 0, while the candidate points in the set of the narrow band are selected from the eight points around the initial starting point. Since the narrow band region is only one cell away from the starting point, it is easy to calculate the distance between the starting point and the eight neighboring points. The propagating speed associated with the grid point (i, j ) is fixed since the speed function depends only on position. The values of the travel time T needed for the wave front to reach the eight neighboring points in the narrow band are initialized by distance/speed. Finally, it is assumed that the travel times to the other points are infinite. After the initialization stage, the points in the narrow band with the smallest value of travel time T are selected for marching forward, and its neighbors with infinite travel time are added to the set of the narrow band. Then, the travel times for the front to propagate to the points which are listed in the NarrowBand queue are recomputed according to Equation (7). As shown in Figure 7, after the initialization stage, the point in the narrow band set with the smallest value of travel time T is selected to march forward. If all the grid points in the workspace have the same propagating speed, the front expanding outward is like a circle. When the front propagates to the grid points occupied by an obstacle, the propagating speed becomes zero to prevent the front from going through this region. As for the front which is far away from the obstacles, its propagating speed will not be disturbed by the obstacles. Consequently, the front near the obstacles becomes concave. It should be noted that two or more points may have the smallest value of travel time T simultaneously, and that their neighbors may overlap each other while the concave front is marching forward. In order to satisfy the Huygens principle (Veselov, 1995; Wymer et al., 1996), the smallest value of travel time T is selected because the front with the smallest T value will propagate across the point first. The marching forward phase terminates when the expanding front reaches the goal. The full processes of the fast marching method can be summarized as follows: INITIALIZATION STAGE. 1. Tag the source point of the front as Alive and set its travel time T = 0. 2. Tag the candidate points around the front as NarrowBand, compute their travel times using Equation (7), and add them into the NarrowBand queue. 3. Tag all other points in the grid space as FarAway.

SPEED PLANNING FOR A MANEUVERING MOTION

35

Figure 7. The procedure of the fast marching algorithm. (a) In the initialization stage, the source is tagged as Alive, i.e., the black circle, and the neighbor grid points are added into the NarrowBand queue, i.e., the grey circles. (b) The marching forward stage. The grey circles with the smallest travel time are chosen, then their values are frozen and are tagged as Alive. Those neighbor grid points which are not tagged as Alive are then added into the NarrowBand queue, and their travel times are computed according to Equation (7).

36

K.-S. HWANG AND M.-Y. JU

MARCHING FORWARD STAGE. 1. Find out the point (imin , jmin ) which has the smallest travel time T in the NarrowBand queue. 2. Tag this point as Alive and remove it from the NarrowBand queue. 3. Tag those points (imin − 1, jmin ), (imin + 1, jmin ), (imin , jmin − 1), (imin , jmin + 1) which are not Alive as NarrowBand, recompute the value of their travel times, and add them into the NarrowBand queue. 4. Return to step 1 until the goal position is reached. In order to record the state of the propagating front with less memory consumption, a time interval is set to execute several marching processes in successive computations. This is done by means of cumulating the smallest values of T . As the cumulative travelling time exceeds the specific time interval, the computation stops and starts to determine the propagating front. The specific time interval is called the frozen time. 4.2. COMPUTE THE VELOCITY PROFILE BY BACKWARD TRACING METHOD Considering the travelling time surface T (x, y) constructed by a propagating interface, the shortest curve on the surface which joins two given points can be derived by means of the calculus of variation (Brechtken-Manderscheid, 1991). Assuming that the travelling time surface, as shown in Figure 8, has a four-fold smooth parametric representation,   x(u ¯ 1 , u2 ) = x1 (u1 , u2 ), x2 (u1 , u2 ), x3 (u1 , u2 ) , where x1 (u1 , u2 ) = u1 , x2 (u1 , u2 ) = u2 , and x3 (u1 , u2 ) = T (u1 , u2 ). A piecewise smooth curve on the surface can be represented, then, as a function of the parameter t:     z¯ (t) = z1 (t), z2 (t), z3 (t) = x¯ u1 (t), u2 (t) , where zi , i = 1, 2, 3, and u1 and u2 are piecewise smooth functions.

Figure 8. Parametric representation of a curve on the surface.

37

SPEED PLANNING FOR A MANEUVERING MOTION

The length L of the curve can be determined by the formula  tb   z (t) dt. L= ta

The square of the length of the vector z¯ (t) is 2 2   2 ∂ x¯  ∂ x¯ z¯ (t) = z¯ (t) · z¯ (t) = u (t) u (t) ∂ui i j =1 ∂uj j i=1

=

2  ∂ x¯ ∂ x¯ · ui (t)u j (t). ∂u ∂u i j i,j =1

¯ ¯ Let gij denote the scalar product of the vectors ∂ x/∂u i and ∂ x/∂u j , then L is represented by 1/2  tb   2   gij u1 (t), u2 (t) ui (t)uj (t) dt. L= ta

i,j =1

The function  F u1 , u2 , u 1 , u 2 = 



2 

1/2 gij (u1 , u2 )u i u j

i,j =1

satisfies the assumptions on the integrand of the parametric problem so that 2 1  gj i u j , F = F j =1 u i

2 1  ∂gj k Fui = u u. 2F j,k=1 ∂ui j k

The parameter t represents arc length if F (ui (t), u i (t)) = 1 for all t. If t is such a parameter, then the Euler differential equation has the following simple form:

2 2      ∂gj k   ∂gij  gij u(t) ¯ uj (t) + u(t) − u(t) ¯ uj (t)u k (t) = 0. ∂u 2∂u k i j =1 j,k=1 Legendre’s necessary condition appears as 2  2 2       gik u(t)u (t)u + gik u(t) ¯ ¯ ui uk
0 for all (u1 , u2 ) ∈ R2 . k i i,k=1

i,k=1

Since the second term is equal to or greater than zero, Legendre’s necessary condition is always satisfied. Thus, a relative minimum exists in the interval [ta , tb ]. It is noticeable that the cumulative travelling time of a propagating interface is monotonically increasing; therefore, when the front touches the goal position, it implies that there is at least one path from the source point to the goal with

38

K.-S. HWANG AND M.-Y. JU

the minimal cumulative travelling time. Since the speed function of the proposed interface propagation method is assigned a value of 1.0 to all nonforbidden regions, the path with the minimal cumulative travelling time is equivalent to the shortest path. Thus, as the shortest curve, which joins the starting point and the goal, on the travelling time surface constructed by the interface propagation method has been derived, the projection of the curve on the STG is the shortest speed path. It is noticeable that the initial speed path for the controllable object is a straight line 45 degrees from the distance axis on an STG, i.e., the robot moves at a constant speed. Based on the concept of the calculus of variation in an obstacle-free space, the minimal distance between two points is a straight line; therefore the shortest smooth path without any collision on an STG implies that the cumulative variation of the moving speed is minimal, while the controllable object is moving to the goal. From a different viewpoint, a speed path with the minimal cumulative variation also implies the minimal acceleration/deceleration control or the minimal force control. From the viewpoint of level set theory, the set T (x, y) = C, constructed by the interface propagation method, is the one of all the points in a 2-dimensional space that can be reached with a minimal cost (arrival time) C. Since the gradient of a function extends in the direction with the greatest change, the gradient at any point is perpendicular to the level set which passes through this point. Therefore, the fact that the minimal geodesic path is perpendicular to the contour of a level set is applied to generating an optimal path. This result is obtained by means of an extension of Gaussian Lemma (Gard, 1987). It is used to track the optimal trajectory by starting from the destination point that has been touched by an equal-time contour, and proceeding backward using the orthogonal property of the optimal path and contours of the level set. Therefore, an optimal speed path can be generated by solving the ordinary differential equation Xt = ∇T

given X(0) = goal position,

until reaching the starting point. Thus, after the interface has propagated all over the workspace, the backward tracing method is then applied to finding the minimal geodesic path between the starting point and the goal, by using the potential field constructed by the travelling time of a propagating interface on the STG. It should be noted that the decent gradient must be bound in the velocity-zone corn to satisfy the acceleration/deceleration constraints. If the gradient is out of the velocity-zone corn, the computed gradient for backward trace is replaced with the slope of the near boundary line of the velocity-zone corn. Then, the generated speed path is also an optimal solution which satisfies the speed constraints. 5. Simulations Two examples are given in this section to demonstrate the performance of the proposed method. In the simulations, it is assumed that moving obstacles do not collide

SPEED PLANNING FOR A MANEUVERING MOTION

39

Figure 9. The dynamic environment for the simulation.

with each other since there is no way to avoid it. In avoiding possible collisions, controllable objects are not allowed to move backward. The unit of distance for the examples is a millimeter (mm). EXAMPLE 1 (Circular path). The test environment is shown in Figure 9. The controllable object (object 1) moves along a given circular path, the radius of which is 60 mm from (60, 120) to (180, 120) at the speed of 10 mm/s. The maximum speed allowed is 30 mm/s and the maximum acceleration/deceleration is 10 mm/s for object 1. Two moving obstacles (objects 2 and 3) move along straight paths crossing the path of object 1 at the speed 10 mm/s and 5 mm/s, respectively. The objects move from (44.27, 4.27) to (177.49, 177.49) and from (112.47, 127.53) to (179.08, 60.92), respectively. In this example, all objects are modeled as disks with a radius of 8 mm. If there are no collisions, the desired arrival times for the objects are all 18.84 s. If there are possible collisions for the planned motions, the velocity of the controllable objects will be controlled under a velocity limit to avoid colliding with other moving obstacles. Figure 10(a) illustrates the STG for object 1 in this simulation. Based on the STG, the proposed method is applied to generating an optimal solution to the velocity planning problem systematically. In the initial phase, the goal position of the STG is set as the source point of the propagating front and the propagating speed functions are set to 0 for the forbidden regions, and 1 for the rest. Figure 10(b) shows the result of applying the interface propagation method to searching for a set of collision-free speed paths on the STG. The velocity profile generated by the proposed approach is shown in Figure 11. The result depicted that the proposed method was able to deal with different types of paths, in addition to straight lines, for robotic applications. EXAMPLE 2 (Multiple controllable moving objects). In this example, we assume that the path of every object is a simple straight line. There are four objects in the

40

K.-S. HWANG AND M.-Y. JU

(a)

(b)

Figure 10. (a) STG of the object 1. (b) Speed path generated by the proposed method.

Figure 11. Altered velocity profile for controllable moving object 1.

environment: two controllable objects (objects 1 and 2) and two moving obstacles (objects 3 and 4). Object 1 has a higher priority than object 2. The starting and the goal positions of object 1 and object 2 are presented by the following homogeneous transformations:     1 0 10 1 0 210 POSe1 = 0 1 110 , and POSS1 = 0 1 110 , 0 0 1 0 0 1     1 0 110 1 0 210 POSe2 = 0 1 POSS2 = 0 1 210 , 10 , respectively. 0 0 1 0 0 1 In addition, the speed and acceleration/deceleration for objects 1 and 2 are restrained within 30 mm/s and 10 mm/s2 , respectively. To simplify this detection

SPEED PLANNING FOR A MANEUVERING MOTION

41

Figure 12. The dynamic environment for the simulation.

process, the collision condition is determined by checking forbidden regions derived from the paths of each object with respect to all other objects. As described before, a forbidden region is determined by a safety distance associated with objects. In our simulation, the safety distances for the controllable objects 1 and 2 are assigned as 10 and 8 mm, respectively. The environment is depicted in Figure 12. In this example, objects 1 and 2 are required to move initially at constant speeds of 10 mm/s and 7.45 mm/s from their starting positions to goal positions. There are also two dynamic obstacles (objects 3 and 4) moving in the same space. To simulate the effect of collision, it is assumed that the safety distances of objects 3 and 4 are 5 and 10 mm, respectively. Objects 3 and 4 move from (10, 210) to (110, 10) and (10, 10) to (210, 210) with constant speeds of 22.36 mm/s and 14.14 mm/s, respectively. If there are no collisions, the desired arrival times for the four objects are 20, 30, 10 and 20 s, respectively. According to the given parameters and formulas, the collision-free trajectory planning problem for a controllable moving object can be converted to an STG with several forbidden regions on it. Since object 1 has a higher priority than object 2, the velocity profile of object 1 is planned first for a collision-free motion. Figure 13(a) illustrates the STG for object 1. After the construction of an STG, the interface propagation method was applied in generating an optimal solution systematically to the velocity planning problem. Figure 13(b) shows the optimal solution with a minimal travelling time. The velocity profile for object 1 generated by the proposed approach is shown in Figure 14. The results show that the semi-continuous velocity profile, where the robot keeps a constant acceleration/deceleration during a control cycle, can successfully eliminate those adjacent points which drive the robot with an infinite acceleration.

42

K.-S. HWANG AND M.-Y. JU

(a)

(b)

Figure 13. (a) STG of the object 1. (b) Speed path generated by the proposed method.

Figure 14. Altered velocity profile for controllable moving object 1.

After the velocity profile of object 1 has been planned, object 1 is in turn regarded as a moving obstacle for the trajectory planning of object 2. The STG and the generated speed path based on this STG for object 2 are shown in Figure 15. The velocity profile of object 2 is shown in Figure 16. The result depicts that a controllable moving object, directed by the proposed method, keeps a constant velocity for sure if no obstacle locates on the original speed path. 6. Conclusion A novel approach to solving the speed alteration problem with speed constraints for maneuvering motions within an environment that contains moving obstacles has been proposed in this paper. The presentation of potential collision is based on a path-velocity decomposition in which each possible collision occupies a rectangular forbidden region in the associated STG. The objective for velocity planning

43

SPEED PLANNING FOR A MANEUVERING MOTION

(a)

(b)

Figure 15. (a) STG of the object 2. (b) Speed path generated by the proposed method.

Figure 16. Altered velocity profile for controllable moving object 2.

is to keep the speed path on the STG from these regions. To tackle this problem, the conventional approaches, such as modified visible graphics, impose some optimization factors on a path in the search procedure. However, since the search space in this case tends to become greater, exponentially proportional to the number of the forbidden regions on an STG. For this reason, an interface propagation method with a fast marching algorithm is tailored for relaxing the complexity. Since the proposed method works on a grid system, it is easy to implement the algorithm in parallel, using each mesh point as a small calculating device which communicates with its neighbors. In this way, the computation time can be reduced drastically. In this method, the forbidden regions and their counterparts are assigned different densities which affect the speed at which the simulated interface front passes over. Following the fastest fronts along the virtual travelling-time axis, a path between the initial and the goal point can be derived. This path is actually a speed path

44

K.-S. HWANG AND M.-Y. JU

with less alteration in sense. Besides, the path induced by this method always has a higher order continuity, i.e., the velocity profile transferred from the speed path could be much smoother. The results indicate an improvement in the formation of a more realistic velocity profile. References Brechtken-Manderscheid, U.: 1991, Introduction to the Calculus of Variations, Chapman & Hall, New York. Chopp, D. L.: 1993, Computing minimal surface via level set curvature flow, J. Comput. Phys. 106, 77–91. De Boor, C.: 1978, A practical guide to splines, Springer, New York. Debreuve, E., Barlaud, M., Aubert, G., and Darcourt, J.: 1998, Attenuation map segmentation without reconstruction using a level set method in nuclear medicine imaging, in: Proc. of Internat. Conf. on Image Processing, pp. 34–38. Fraichard, T.: 1993, Dynamic trajectory planning with dynamic constraints: A ‘state-time space’ approach, in: Proc. of Internat. Conf. on Intelligent Robots and Systems, pp. 1393–1400. Fraichard, T. and Scheuer, A.: 1994, Car-like robots and moving obstacles, in: Proc. of Internat. Conf. on Robotics and Automation, pp. 64–69. Gard, T. C.: 1987, Introduction to Stochastic Differential Equations, Marcel Dekker, New York. Hwang, K. S., Chao, H. J., and Lin, J. H.: 1999, Collision-avoidance motion planning admit multiple moving objects, J. Inform. Sci. Engrg. 15, 715–736. Kant, K. and Zucker, S. W.: 1986, Toward efficient trajectory planning: The path-velocity decomposition, Internat. J. Robotics Research 5, 72–89. Leymarie, F. and Levine, M. D.: 1992, Simulating the grassfire transformation using an active contour model, IEEE Trans. Pattern Anal. Machine Intell. 14, 56–75. Liu, Y. H. and Arimoto, S.: 1994, Computation of the tangent graph of polygonal obstacles by moving-line processing, IEEE Trans. Robot. Automat. 10, 823–830. Lozano-Pérez, T.: 1981, Spatial planning: a configuration space approach, IEEE Trans. Systems Man Cybernet. 11, 681–698. Lozano-Pérez, T. and Wesley, M. A.: 1979, An algorithm for planning collision-free paths among polyhedral obstacles, Commun. ACM 22, 560–570. Malladi, R. and Sethian, J. A.: 1996, A unified approach to noise removal, image enhancement, and shape recovery, IEEE Trans. Image Proc. 5, 1554–1568. Malladi, R., Sethian, J. A., and Vemuri, B. C.: 1995, Sharpe modeling with front propagation: A level set approach, IEEE Trans. Pattern Anal. Machine Intell. 17, 158–175. O’Donnell, P. A. and Lozano-Pérez, T.: 1989, Deadlock-free and collision-free coordination of two robot manipulators, in: Proc. of IEEE Internat. Conf. on Robotics and Automation, pp. 484–489. Qin, F., Luo, Y., Olsen, K. B., Cai, W., and Schuster, G. T.: 1992, Finite-difference solution of the eikonal equation along expanding wavefronts, Geophysics 57, 478–487. Sethian, J. A.: 1985, Curvature and the evolution of fronts, Comm. Math. Phys. 101, 486–499. Udupa, S.: 1977, Collision detection and avoidance in computer controller manipulators, PhD Thesis, California Institute of Technology. Van Trier, J. and Symes, W. W.: 1991, Upwind finite-difference calculation of travel-times, Geophysics 56, 812–821. Veselov, A. P.: 1995, Huygens’ principle and integrable systems, Phys. D 87, 9–13. Warren, C. W.: 1990, Multiple robot path coordination using artificial potential fields, in: Proc. of IEEE Internat. Conf. on Robotics and Automation, pp. 500–505. Wymer, S. A., Akhlesh, A., and Engel, R. S.: 1996, The Huygens’ principle for flow around an arbitrary body in a viscous incompressible fluid, Fluid Dynamics Res. 17, 213–223.