Robot Motion Planning in Dynamic Environments Paolo Fiorini
Jet Propulsion Laboratory, California Institute of Technology, Pasadena, CA 91109 USA
email:
[email protected]
Zvi Shiller
Department of Mechanical, Nuclear and Aerospace Engineering, University of California, Los Angeles, Los Angeles, CA 90024 USA email:
[email protected]
Abstract This paper presents a method for computing the motions of a robot in dynamic environments, subject to the robot dynamics and its actuator constraints. This method is based on the concept of Velocity Obstacle, which de nes the set of feasible robot velocities that would result in a collision between the robot and an obstacle moving at a given velocity. The avoidance maneuver at a speci c time is thus computed by selecting robot's velocities out of that set. A trajectory consisting of a sequence of avoidance maneuvers at discrete time intervals is generated by a search of a tree of avoidance maneuvers. An exhaustive search computes near minimum-time trajectories, whereas a heuristic search generates feasible trajectories for on-line applications. These trajectories are compared to the optimal trajectory computed by a dynamic optimization that minimizes motion time, subject to robot dynamics, its actuator limits and the state inequality constraints due to the moving obstacles. This approach is demonstrated for planning the trajectory of an automated vehicle in an Intelligent Vehicle Highway System scenario.
1 Introduction This paper addresses the problem of motion planning in dynamic environments. Typical examples of dynamic environments include manufacturing tasks in which robot manipulators track and retrieve parts from moving conveyers, and air, sea,
and land trac, where aircraft, vessels and vehicles avoid each other while moving towards their destination. Motion planning in dynamic environments is considerably more dicult than the widely studied static problem, since it requires the simultaneous solution of the path planning and of the velocity planning problems. Path planning involves the computation of a collision free path from start to goal without considering robot dynamics. Velocity planning, on the other hand, involves the computation of the velocity pro le along a given path, satisfying system dynamics and actuator constraints. In addition, motion planning in static environments can be guaranteed to nd a solution if one exists at time t0 , whereas motion planning in dynamic environments is essentially intractable, [38], [9], i.e. the solution at t0 may not exist at a later time because of the evolution of the environment. Previous methods consisted of a graph search in a con guration-time space of the robot [12, 13], and checking for intersection of the swept volumes of the robot and obstacles in a Cartesiantime space [5, 6, 7, 8]. Velocity constraints were considered in [37] for the solution of the asteroid problem, and acceleration constrains were satis ed in [10] and in [35] for planning for a point mass. Approximate dynamic constraints were satis ed in [25, 26] by introducing the path-velocity decomposition method, and by limiting slope, direction and curvature of the trajectory in a position-time plane. A family of adjacent paths is
developed in [17] to account for obstacles blocking the initial path, thus computing the complete trajectory by connecting di erent path segments. In [20] a network of possible paths is searched to compute the time-minimal trajectory. Dynamic constraints were used in [21, 22] to de ne the collision front, and in [18, 19] to introduce the concept of transient obstacles, also applicable to dynamic environments. To date, on-line dynamic planning has been treated by emphasizing reasoning and decision making [38], or by creating arti cial potential elds around the obstacles [27]. The time-optimal motion planning problem in static environments has been treated previously using parameter optimizations, representing the trajectory as a polynomial in time and accounting for obstacles using a penalty function [24], and representing the path by a cubic spline [40], [1], computing the motion time along the path with an ecient method developed in [1] and in [41]. Time optimal motion planning of cooperating robots moving along speci ed paths has been studied in [29, 41]. In this paper, we develop an ecient method for computing the trajectories of a robot moving in a time-varying environment. It utilizes the concept of Velocity Obstacle (VO), which represents the robot's velocities that would cause a collision with an obstacle at some future time. An avoidance maneuver is computed by selecting velocities that are outside of the velocity obstacle. To ensure that the maneuver is dynamically feasible, robot dynamics and actuator constraints are mapped into the robot velocity space. A trajectory consists of a sequence of such avoidance maneuvers, computed by searching over a tree of avoidance maneuvers generated at discrete time intervals. For on-line applications, the tree is pruned using a heuristic search designed to achieve a prioritized set of objectives, such as avoiding collisions, reaching the goal, maximizing speed, or computing trajectories with desirable topology. The solutions computed with this method are conservative, since they exclude trajectories that, although feasible, include avoidance maneuvers violating the velocity obstacle. To evaluate the quality of these trajectories, they are compared to the trajectories computed using a dynamic optimization, which are not bound by the velocity obstacle approach. The optimization used here is a steepest descent [2, 11], modi ed to include the state-dependent inequality constraints due to the
moving obstacles. The initial guess of the optimization is the trajectory generated by the search over the maneuver tree. The advantages of this approach are multi-fold: i) it permits an ecient geometric representation of potential avoidance maneuvers of the moving obstacles, ii) any number of moving obstacles can be avoided by considering the union of their VO's, iii) it uni es the avoidance of moving as well as stationary obstacles, and iv) it allows for the simple consideration of robot dynamics and actuator constraints. This paper is organized as follows. Section 2 de nes the Velocity Obstacle, the avoidance velocities, and describes the representation used to compute the trajectories. Section 3 describes the dynamic optimization used to compute the time optimal trajectories. Examples of trajectories, and of their corresponding time optimal solutions are presented in Section 4.
2 The Velocity Obstacle
The Velocity Obstacle (VO) is an extension of the Con guration Space Obstacle [32] to a timevarying environment. It consists of the velocities of the robot that will cause a collision between the robot and the obstacles at some future time. Although this concept is valid for general robots and obstacles, in this paper we restrict our analysis to circular robots and obstacles in the plane. In this section we rst de ne the VO concept and then we combine it with the dynamic constraints of the robot to compute avoidance velocities that are also dynamically feasible. The corresponding avoidance maneuvers are used to build a tree that represents, within the given temporal resolution, all the avoidance trajectories generated to satisfy the constraints represented by the velocity obstacles.
2.1 The Velocity Obstacle
The VO is illustrated using the scenario shown in Figure 1, where two circular objects, A and B1 , are shown at time t0 with velocities vA and vB1 . Circle A represents the robot, and circle B1 represents the obstacle. The velocities and positions of A and B1 were chosen so that A and B1 will collide at some time t1 ; (t1 > t0 ), provided that vA and vB1 do not change. To compute the VO, we rst represent B1 in the Con guration Space of A by reducing A to
λ AB 1
B1
Velocity of B 1
^ B1
V B1
λ
V B1
r λ
CC A,B1 V A,B1
V A A
Velocity of A
^ A
Figure 1: The robot and a moving obstacle.
f
-VB 1 V A
Figure 2: The Collision Cone CCA;B1 .
the point Ab, and enlarging B1 by the radius of the collision cone CCA;B1 by vB1 , as shown in A to the circle Bc1 [31], and then we attach the Figure 3 [15]. The Velocity Obstacle V O is then velocity vectors to the position of Ab and to the de ned as: center of Bc1 , respectively. By considering the relative velocity V O = CCA;B1 vB1 (3)
vA;B1 = vA ? vB1
(1) and by assuming that A and B1 maintain their current velocities, a collision between Ab and Bc1 will occur at some future time t1 > t0 if the line A;B1 of the relative velocity vA;B1 intersects Bc1 . In fact, any relative velocity that lies between the two tangents to Bc1 f and r will cause a collision between A and B1 . Therefore, we de ne the Collision Cone, CCA;B1 , as the set of colliding relative velocities between Ab and Bc1 or:
CCA;B1 = fvA;B1 j A;B1 \ Bc1 6= ;g
(2)
This cone is the planar sector with apex in Ab, bounded by the two tangents f and r from Ab to Bc1 , as shown in Figure 2. The collision cone thus partitions the space of relative velocities into colliding and avoiding velocities. The relative velocities, vA;B1 , lying on the boundaries of CCA;B1 represent tangent maneuvers that would graze obstacle B1 . The collision cone is speci c to a particular pair of robot/obstacle. To consider multiple obstacles, it is useful to establish an equivalent partition of the absolute velocities of A. This is done simply by adding the velocity of B1, vB1 , to each velocity in CCA;B1 or, equivalently, by translating
where is the Minkowski vector sum operator. Thus, the VO partitions the absolute velocities of A into avoiding and colliding velocities. Velocities on the boundaries of V O would result in A grazing B1 , since the corresponding relative velocities lie on the boundary of the collision cone CCA;B1 . Note that the VO of a stationary obstacle is identical to its relative velocity cone, since then vB1 = 0. To avoid multiple obstacles, the V O's of each obstacle are combined into a single velocity obstacle: V O = [mi=1 V Oi (4) where m is the number of obstacles. The VO assumes that the velocity of B1 remains constant. To account for variable velocities, V O is recomputed at speci ed time intervals. The assumption of circular robot and obstacles reduces the dimension of the con guration space, and thus greatly simpli es the computation of the V O. It also xes the shape of the con guration space obstacles, which are generally functions of their positions in the robot's work space. For general manipulators, the V O must be periodically recomputed to account for the time varying con guration space obstacles [13].
^ B1
^ B1
VB 1
VO
VB 1
VO
B1 RV A
H
V A ^ A VB 1
Figure 3: The velocity obstacle V O.
2.2 The Avoidance Maneuvers
The velocities reachable by robot A at a given state over a given time interval t are computed by transforming the control constraints into bounds on the robot acceleration. The set of feasible accelerations at time t0, FA(t0), is de ned as;
FA(t0) = fx j x = f (x; x_ ; u); u 2 U g (5) where f (x; x_ ; u) represents the dynamics of the robot, u are the actuator e orts, U is the set of admissible controls, and x is the position vector de ned earlier. Note that the feasible acceleration range of a two degree-of-freedom system with decoupled actuator limits is a parallelogram [39]. The set of reachable velocities, RV(t0 + t), over the time interval t is thus de ned as: RV(t + t) = fv j v = vA(t0 ) t FA(t0)g (6) The set of reachable avoidance velocities, RAV, is de ned as the di erence between the reachable velocities and the velocity obstacle: RAV(t0 + t) = RV(t0 + t) V O(t0) (7) where denotes the operation of set di erence. A maneuver avoiding obstacle B1 is thus computed by selecting any velocity in RAV. Figure 4 shows schematically the reachable velocity set RAV consisting of two disjoint closed sets, Sf and Sr , corresponding respectively to all feasible front and rear avoidance maneuvers. For multiple obstacles, the RAV may consist of multiple disjoint subsets.
M
V A ^ A
B1
K VB 1
RAV A
Sf
Sr
L
Figure 4: The reachable avoidance velocities.
2.3 Computing the avoidance trajectories
The trajectory that avoids static and moving obstacles, reaches the goal, and satis es the robot's dynamic constraints is computed as a discrete sequence of elementary avoidance maneuvers, selected by a global search over the tree of all feasible maneuvers at speci ed time intervals. Alternatively, the global search may be reduced to a heuristic search for on-line applications, where the trajectories of the moving obstacles are not known a-priori, but are rather acquired in realtime. These two approaches are discussed next.
2.3.1 Global Search
We represent the state space of the robot by a tree of avoidance maneuvers at discrete time intervals. The nodes on this tree correspond to the positions of the robot at discrete times ti. The operators expanding a node at time ti into its successors at time ti+1 = ti + T are the velocities in the reachable avoidance velocity set RAV. The edges correspond to the avoidance maneuvers at those positions [34]. The search tree is then de ned as follows: ni;j = fxi;j = (xj (ti); yj (ti)); (8) vi;j = (vj;x(ti); vj;y (ti))g oi;j;l = fvl j vl 2 RAVj (ti)g (9) ej;k = f(ni;j ; ni+1;k ) j (10) ni+1;k = ni;j + (oi;j;l t)g where ni;j if the j th node at time ti , RAVj (ti) is the reachable velocity set computed for node ni;j ,
RAV
n i+1,h
i,j
e
o o
o
i+1,h,1
o
v
i+1,h,2
v
o
i,j,k
e
i+1,k,1
VO
VO
i,j,h
n i,j
RAV
i+1,h
n i+1,k
o
RAV
A
A RAV
A
A (a)
j,k
Goal
Goal
j,h
Max angle
(b)
i+1,k,2
i+1,k
Figure 5: Tree representation for the global search.
oi;j;l is the lth operator on node j at time ti, and ej;k is the edge between node nj at time ti, and node nk at time ti+1 . The tree of all feasible avoidance maneuvers is constructed as follows. At time ti, the avoidance set RAV, corresponding to node ni;j , is discretized by a grid. Then, the velocities corresponding to each node on the grid are used to compute the edges emanating from node nij . The positions reached by the robot at the end of each maneuver over the interval T are the successors of node ni;j . A node ni;j is completely expanded when all the operators oi;j;l have been applied and all the edges emanating from ni;j have been examined. The resulting tree has a constant time interval T between nodes, a variable branch number that is a function of the shape of each RAV, and it can be searched using standard techniques [36]. Figure 5 shows schematically a subtree of some avoidance maneuvers. Since the avoidance maneuvers are selected based on the velocity obstacles V O, they are never on a collision course with any of the considered obstacles, as discussed earlier. This excludes feasible trajectories that might, part of the time, be on a collision course with some of the obstacles. However, such trajectories may be generated by either considering only obstacles with imminent collisions, or by re ning the trajectory using a dynamic optimization, as discussed later in Section 3.
2.3.2 Heuristic Search For on-line applications, or when only incomplete information about the environment is available,
Figure 6: a: TG strategy. b: MV strategy. the maneuver tree can be constructed incrementally using heuristic rules designed to satisfy the prioritized set of goals embedded in the formulation of the velocity obstacle. The survival of the robot can be guaranteed by selecting the avoidance velocities RAV; the target can be reached by selecting velocities that point towards the destination; the motion time can be minimized by choosing the highest velocity available; and the desired trajectory structure can be selected by choosing an appropriate sequence of front and rear avoidance maneuvers [14]. We thus propose the following basic heuristics: (a) TG (to goal), choose the highest avoidance velocity along the line to the goal, as shown in Figure 6-a; (b) MV (maximum velocity), select the maximum avoidance velocity within some speci ed angle from the line to the goal, as shown in Figure 6-b. Other heuristics may combine the above strategies in order to better satisfy the prioritized goals.
3 Dynamic Optimization The trajectory computed by the heuristic search is evaluated o -line by a dynamic optimization. This optimization is formulated using Mayer's notation for the performance index J : min J (11) u(t)2U
J = (x(tf ); tf ) = tf (12) subject to the following kinematic and dynamic constraints: 0 = ?(x(t0); t0 ) (13) 0 = (x(tf ); tf ) (14) :
[n
i=1
[Si (x(t); t) = 0]
x_ = F (x; u) = f (x) + g(x)u
(15) (16)
U = fu j ui(min) ui ui(max)g (17) where (13) is the initial manifold, (14) is the terminal manifold, (15) represents the time-varying obstacles, (16) is the robot dynamics, and (17) represents the admissible controls. State constraints due to the presence of obstacles (15) are di erentiated with respect to time until they become explicit in the controls u, and then appended as state dependent control constraint to the Hamiltonian [4, 11]. The number of di erentiations of each constraint represents the order p of that constraint. This approach requires an additional tangency constraint at the entry point of the constrained arc [42]. This may over-constrain the problem for constraints of order higher than two [23]. In the case of a single obstacle, the state constraint (15) is replaced by the tangency condition, denoted 1 , and the control equality constraint, denoted 2 :
The adjoint equations for the Lagrange multipliers are given by:
T _ = ? @@Hx @ @
T T (tf ) = @x + @x
(22)
t
(23)
f
which exhibit the discontinuity at the entry point of the constraint given by [11]: Tt?1 = Tt+1 + T @x@ (t1 ) (24) 1 When the constraint (19) is active, the minimization of the Hamiltonian for an autonomous system is equivalent to satisfying:
Hu ((t); x(t); u (t)) = 0 t0 t tf (25)
and the adjoint equations for the constrained arcs become: 0 S (x(t); t) = 0 1 T = ?T @ f (x) + @ g(x)u ? _ (26) _ C B S (x(t); t) = 0 C @x @x 1 : B t = t (18) C B 1 . @'(x; u) ?1 @'(x; u) # .. A @ g(x) @ u S (p?1) (x(t); t) = 0 @x 2 : S (p) (x(t); u(t); t) = 0; t1 t t2 (19) In summary, the trajectory minimizing the perThen, the admissible control set U for the optimal formance index J = tf is characterized by the following: (i) its controls u are in the admissible set control u(t) becomes: U , (ii) the states satisfy the terminal manifolds ? S (p) (x(t); u(t); t) = 0 for S (x; t) = 0 at t0 and at tf , (iii) the co-state equations are U : U : umin (x) u umax (x) described by (22) on the free arcs and by (26) (20) on the constrained arcs, with a discontinuity at the junction of the two given by (24), and (iv) the Hamiltonian is minimized over the entire in3.1 The Necessary Optimality terval. These conditions are satis ed by the trajectory computed by the numerical method deConditions scribed next [14]. The optimal control u (t) in the interval t0 t tf , that generates the optimal solution, x(t), 3.2 Numerical Computation minimizes J , and satis es the xed terminal manifolds ? and , is computed by satisfying the nec- The optimal trajectory is computed numerically essary conditions of Pontryagin Minimum Princi- by appending the constraints to the performance ple [3, 4, 30]. index J via appropriate arrays of Lagrange mulThe Hamiltonian function H for this problem tipliers, and by computing the corrections to the is de ned as: controls that drive to zero the di erential of the augmented performance index dJb [14]. This difH(; x; u) = T (f (x)+g(x)u)+T '(x; u) (21) ferential is then derived as a function of the variations in the control switches, assuming a bangwhere and are vectors of Lagrange multipli- bang solution [43]. Singular arcs are approxiers, and '(x; u) is the set of active control con- mated by a nite number of switches, generating straints at time t, t0 t tf . a sub-optimal solution [33].
The di erential of the augmented performance index is formed by computing the di erentials of J , and 1 independently, then including the e ects of 1 on dJ and d , and nally combining all into dJb. The nal form of the augmented performance index is: @ @
T b (27) dJ = @t + @t + H dtf t +
Z t?1 t0
Huud +
Zt
?1 ( + I + I ) = ?I? (30) ?1 1 I + 2 I
= ? 1 I
+ 1 I 1 I
! 1?1
T A +3 I
+ 1b ddt ddt
? + 1 I ? 1 I 1 I ?1 ?t + 1 I +
! 2 I + 3 I + 1 d d b dt dt t (31)
f
f
t+1
The values of dtij and dtf depend on multipliers and . Their values are computed by requiring that di erentials d (t1) and d (tf ) be reduced by given quantities and . The values for the multipliers and are then:
Huud
where the Hamiltonian is given by H = T F + ' as in equation (21). The Lagrange multiplier is de ned as T = T + T T + T T since the constraints 1 are not a ected by the state after t1 , and , are two constant Lagrange multipliers. Bang-bang controls can only take one of two values M or m and the variation in u is rewritten as ui = ( M ? m ) sgn(dti) where sgn is the signum function. If ss;r indicates the number of switches in the s segment of the trajectory for the rth input, and m indicates the number of elements in u, then dJb for bang-bang controls becomes: s1 m X X b (Hu )t uidtij dJ =
f
f
where:
m s l Ih;k = X X i=1 j =1 l;i
@ F w?1 @ F Th @u i ii @ui
T
k
! t
ij
with
h = 1 ; ; k = 1 ; ; ; l = 1; 2; 3 The index l indicates the segments of the trajectory before, on, and after the state constraint. Index i on the constraint boundary represents only the independent controls.
3.3 Generating the Initial Guess
The trajectory computed by the heuristic search (28) consists of a sequence of straight line segments i=1 j =1 and velocities assigned to each segment. Since s2 m X the velocity changes abruptly between segments, X (Hu )t uidtij + we smooth it by a spline interpolation. Each trai=1 j =1 jectory segment i is divided into the two halves s3 m X i1 and i2 , and the spline interpolation is applied X + (Hu )t uidtij to the trajectory segments i2 and (i + 1)1. This i=1 j =1 replaces the velocity discontinuities by a polyno @ mial blend, chosen as a third order Hermite spline @
+ @t + T @t + H dtf [16]. t The bang-bang controls for the spline interpoThis di erential is minimized by choosing the lation are computed as follows. First, the actuator e orts are computed from the accelerations steepest descent increments as [4]: of the spline interpolation using the inverse sys(Hu )t tem dynamics. Then, the smooth actuator e orts (29) dtij = ? w u are approximated by bang-bang controls, so that ii i the trajectory obtained by integrating the conT @
+ dtf = ? 1b H + @ trols has the same shape as the trajectory com@t @t t puted by the search. The optimal trajectory is a with wii and b being suitable positive values. local minimum, in the vicinity of the initial guess, ;i
i
ij
;i
i
ij
i
ij
;i
f
i
ij
f
robot robot
vehicle 2 vehicle 2
vehicle 1 vehicle 1
Exit
Figure 7: Example of trajectory problem.
Exit
Figure 9: Trajectory computed by global search.
30:0 m=s; vy = 0:0 m=s), and the initial velocity of vehicle 1 is (vx = 23 m=s; vy = 0:0 m=s). The velocities are represented as bars attached to the center of each circle. The time interval T between nodes in the search tree is chosen arbitrarily as T = 1s. The trajectories are computed using the global search and the heuristic search with the TG and MV-TG In these examples, the computation of trajecto- strategies. The solution computed by the MV-TG ries is demonstrated for the Intelligent Vehicle strategy is then compared to the optimal soluHighway System (IVHS) scenario shown in Fig- tion computed by the dynamic optimization. In ure 7. The goal of the robotic vehicle in the left- the following pictures, the circles representing the most lane is to reach the exit to the right without robot and the obstacles are displayed at :5 s intercolliding with the other two vehicles. The initial vals, and the robot is represented by the darker velocity of the robot and of vehicle 2 is (vx = circle.
computed by optimizing the switching times. For the purpose of the following examples, the dynamics of the robot are modeled by two double integrators.
4 Examples
robot robot
vehicle 2 vehicle 2
vehicle 1 vehicle 1
Exit Exit
Figure 8: Trajectory computed with the TG strategy.
Figure 10: Trajectory computed with MV-TG strategies.
1.5 Bang-Bang Ax
1 0.5
Start
0
Robot
-0.5 -1 -1.5 0
.5
1.0
1.5
2.0
2.5
3.0
3.5
4.0
4.5
5.0 t
Vehicle 2
1.5 Bang-Bang Ay
1
Vehicle 1
0.5 0
Exit
-0.5 -1 -1.5 0
.5
1.0
1.5
2.0
2.5
3.0
3.5
4.0
4.5
5.0 t
Figure 11: The Bang-Bang approximation of the controls of the initial guess. The grey circles represent the position of the robot and of the obstacles when the robot crosses the path of the obstacles.
4.1 The Trajectories computed using the Velocity Obstacles The trajectory shown in Figure 9 is computed using the Depth First Iterative Deepening algorithm [28]. This algorithm returns the fastest path to the target the rst time it reaches the target. In the example, the RAV sets have been discretized by considering four points on each side of the boundary, and the maximum feasible velocity in the direction to the goal. The search has been expanded to 4:5 s and a depth of 5 levels . The total motion time of this solution is of 3:5 s. Using the TG strategy resulted in the trajectory shown in Figure 8. Along this trajectory, the robot slows down and lets vehicle 2 pass, and then speeds up towards the exit, behind vehicle 1. The total motion time for this trajectory is 6:07 s. The trajectory computed using both MV and TG strategies is shown in Figure 10. Along this trajectory, the robot rst speeds up to pass vehicle 2, then slows down to let vehicle 1 pass on, and then speeds up again towards the goal. The motion time for this trajectory is 4:85 s. This trajectory was computed by using the MV heuristics for 0 t 2:0 s and the TG heuristics afterwards.
Figure 12: Initial guess of the dynamic optimization
4.2 The Optimal Trajectory Figure 11 shows the bang-bang approximation of the controls for the trajectory shown in Figure 10. Figure 12 shows the initial guess of the dynamic optimization, obtained by integrating the bang-bang controls. The e ects of the approximation show in the di erent terminal position of the robot, far from the target, and in collision with vehicle 1. This collision is represented in Figure 12 by a few bars between time t = 2:5s and time t = 3:0s, indicating the depth of the contact between the robot and vehicle 1. This trajectory resembles the trajectory of Figure 10, namely the type of maneuvers used in avoiding the obstacles, and the time of the avoidance of vehicle two, at approximately t = 2:5s. By using this trajectory as the initial guess, the dynamic optimization converges to the local minimum in the vicinity of the trajectory computed using the velocity obstacles, as shown in Figure 14. The optimal controls are shown in Figure 13. The optimal solution of Figure 14 has the same key features of the heuristic trajectory of Figure 10, thus allowing for a meaningful comparison between the trajectory computed by the heuristic search and the corresponding optimal solution. The motion time of the optimal trajectory is 4:6 s, only slightly lower than the time of the heuristic solution of 4:85 s. The optimal trajectory violates at some points the velocity obstacle due to vehicle 1, which accounts for the shorter motion time.
1.5 Ax of the solution
1 0.5 0 -0.5
Start
-1 -1.5 0
.5
1.0
1.5
2.0
2.5
3.0
3.5
4.0
4.5
Robot
t
1.5 Ay of the solution
1
Vehicle 2
0.5 0 -0.5
Vehicle 1
-1 -1.5 0
.5
1.0
1.5
2.0
2.5
3.0
3.5
4.0
4.5 t
Exit
Figure 13: The optimal Bang-Bang controls.
5 Conclusion A novel method for planning the motion of a robot moving in a time-varying environment has been presented. It is signi cantly di erent from currently available planning algorithms, since it simultaneously computes the path and velocity pro le that avoid all static and moving obstacles and satisfy the robot's dynamic constraints. The method consists of computing, for every obstacle, its corresponding velocity obstacle, which is the set of colliding velocities between the obstacle and the robot. Then, by subtracting it from the reachable velocities of the robot, the set of reachable avoidance velocities is formed, which consists of all the velocities that avoid the obstacles and satisfy the robot's dynamic constraints. A search space is then formed by representing the state space of the robot by a tree of avoidance maneuvers. A global search over the tree yields trajectories that minimize a selected performance index, such as motion time or traveled distance. The solutions computed with this method are conservative, since each maneuver avoids all obstacles, irrespectively of their expected collision time, and they may exclude trajectories that, although feasible, violate the velocity obstacle in some interval. For on-line applications, the tree is pruned using one of several heuristic strategies, aimed at satisfying a prioritized list of goals, such as the survival of the robot, reaching the target, and minimize motion time. The quality of the heuristic trajectories is evaluated by comparing them to the optimal trajectories computed by dynamic optimization. The velocity obstacle method can be extended to three dimensional environments, by modeling the robots and the obstacles with spheres. The method is demon-
Figure 14: Optimal solution. strated for planning the trajectory of an automated vehicle in an Intelligent Vehicle Highway System scenario. The main advantages of the velocity obstacle approach include the ecient geometric representation of maneuvers avoiding any number of moving and static obstacles, and the simple consideration of robot dynamics and actuator constraints. This approach, therefore, makes it possible to compute on-line safe and feasible trajectories in dynamic environments, such as Air Trac control, Intelligent Highway Systems, and Harbor Trac management.
6 Acknowledgment The research described in this paper has been partially carried out at the Jet Propulsion Laboratory, California Institute of Technology, under contract with the National Aeronautic and Space Administration.
References [1] J.E. Bobrow, S. Dubowsky, and J.S. Gibson. Time-optimal control of robotic manipulators along speci ed paths. The International Journal of Robotics Research, 4(3):3{17, Fall 1985. [2] A.E. Bryson and W.F. Denham. A steepestascent method for solving optimum programming problems. ASME Journal of Applied Mechanics, (29):247{257, June 1962.
[3] A.E. Bryson, W.F. Denham, and S.E. Dreyfus. Optimal programming problems with inequality constraints i: Necessary conditions for extremal solutions. AIAA Journal, 1(11):2544{2550, November 1963. [4] A.E. Bryson and Y.C. Ho. Applied Optimal Control. Hemisphere Publishing Corp., New York, NY, 1975. [5] S. Cameron. A study of the clash detection problem in robotics. In IEEE International Conference on Robotics and Automation, pages 488{493, St. Louis, MO, March 25-28 1985. [6] S. Cameron. Ecient intersection tests for objects de ned constructively. The International Journal of Robotics Research, 8(1):3{ 25, February 1989. [7] S. Cameron. Collision detection by fourdimensional intersection testing. IEEE Journal of Robotics and Automation, 6(3):291{ 302, June 1990. [8] S.A. Cameron. Modelling Solids in Motion. PhD thesis, Edinburgh, UK, 1984. [9] J. Canny and J. Reif. New lower bound techniques for robot motion planning problems. In 28th IEEE Symposium on Foundation of Computer Science, 1987. [10] J. Canny, J. Reif, B. Donald, and P. Xavier. On the complexity of kinodynamic planning. In 29th IEEEE Symposium on Foundation of Computer Science, 1988. [11] A.E. Denham, W.F.and Bryson. Optimal programming problems with inequality constraints ii: Solution by steepest ascent. AIAA Journal, 2(2):25{34, January 1964. [12] M. Erdmann and T. Lozano-Perez. On multiple moving objects. In IEEE International Conference on Robotics and Automation, pages 1419{1424, San Francisco, CA, April 7-10 1986. [13] M. Erdmann and T. Lozano-Perez. On multiple moving objects. Algorithmica, (2):477{ 521, 1987. [14] P. Fiorini. Robot Motion Planning among Moving Obstacles. PhD thesis, University of California, Los Angeles, January 1995.
[15] P. Fiorini and Z. Shiller. Motion planning in dynamic environments using the relative velocity paradigm. In IEEE International Conference of Automation and Robotics, volume 1, pages 560{566, Atlanta, GA, May 1993. [16] J.D. Foley, A. van Dam, S.K. Feiner, and J.F. Hughes. Computer Graphics. AddisonWesley Publishing Company, Reading, MA, 1990. [17] Th. Fraichard and C. Laugier. Path-velocity decomposition revisited and applied to dynamic trajectory planning. In IEEE International Conference of Automation and Robotics, volume 2, pages 40{45, Atlanta, GA, May 1993. [18] K. Fujimura. On motion planning amidst transient obstacles. In IEEE International Conference on Robotics and Automation, pages 1488{1493, Nice, France, May 1992. [19] K. Fujimura. Motion planning amid transient obstacles. International Journal of Robotics Research, 13(5):395{407, October 1994. [20] K. Fujimura. Time-minimum routes in time-dependent networks. IEEE Transaction on Robotics and Automation, 11(3):343{351, June 1995. [21] K. Fujimura and H. Samet. Time-minimal paths among moving obstacles. In IEEE International Conference on Robotics and Automation, pages 1110{1115, Scottsdale, AZ, May 1989. [22] K. Fujimura and H. Samet. Motion planning in a dynamic domain. In IEEE International Conference on Robotics and Automation, pages 324{330, Cincinnati, OH, May 1990. [23] D.H. Jacobson, M.M. Lele, and J.L. Speyer. New necessary conditions of optimality for control problems with state-variable inequality constraints. Journal of Mathematical Analysis and Applications, 35(2):255{284, 1971. [24] D.W. Johnson and E.G. Gilbert. Minimum time robot planning in the presence of obstacles. In IEEE Conference on Decision and Control, pages 1748{1753, Ft. Lauderdale, FL, December 1985.
[25] K. Kant and S.W. Zucker. Towards ecient [37] J. Reif and M. Sharir. Motion planning in the trajectory planning: the path-velocity depresence of moving obstacles. In 25th IEEE composition. The International Journal of Symposium on the Foundation of Computer Robotic Research, 5(3):72{89, Fall 1986. Science, pages 144{153, 1985. [26] K. Kant and S.W. Zucker. Planning colli- [38] J.C. Sanborn and J.A. Hendler. A model of reaction for planning in dynamic environsion free trajectories in time-varying environments. International Journal of Arti cial Inments: a two level hierarchy. In IEEE Intelligence in Engineering, 3(2):95{101, April ternational Conference on Robotics and Au1988. tomation, pages 1644{1649, Raleigh, NC, 1988. [39] Z. Shiller and Dubowsky. Time optimal paths and acceleration lines of robotic ma[27] O. Khatib. A uni ed approach for motion nipulators. In The 26th IEEE Conference and force control of robot manipulators: the on Decision and Control, Los Angeles, CA, operational space formulation. IEEE TransDecember 1987. action on Robotics and Automation, 3:43{53, 1987. [40] Z. Shiller and S. Dubowsky. Robot path planner with obstacles, actuators, gripper [28] R. Korf. Depth- rst iterative-deepening: An and payload constraints. The International optimal admissible tree search. Arti cial InJournal of Robotics Research, 8(6):3{18, Detelligence, (27):97{109, 1985. cember 1989. [29] B.H. Lee and C.S.G. Lee. Collision-free mo- [41] K.G. Shin and N.D. McKay. Open loop minimum time control of mechanical manipulation planning of two robots. IEEE Transactors. In American Control Conference, pages tions on System Man and Cybernetics, SMC1231{1236, June 1984. 17(1):21{32, January-February 1987. [30] G. Leitman. An Introduction to Optimal [42] J. Speyer. Optimization and Control of Nonlinar Systems with In ight Constraints. PhD Control. McGraw-Hill Book Co., New York, thesis, Harvard, February 1968. NY, 1966. Weinreb and A.E. Bryson. Optimal con[31] T. Lozano-Perez. Spatial planning: a con- [43] A. trol of systems with hard control bounds. guration space approach. IEEE TransacIEEE Transactions on Automatic Control, tion on Computers, C-32(2):108{120, FebruAC-30(d116), November 1985. ary 1983. [32] T. Lozano-Perez and M. A. Wesley. An algorithm for planning collision-free paths among polyhedral obstacles. Communications of the ACM, 22(10):560{570, October 1979. [33] E.B. Meier. An Ecient Algorithm for Bang-Bang Control Systems Applied to a Two-Link Manipulator. PhD thesis, Stanford, December 1987. [34] N Nilsson. Principles of Arti cial Intelligence. Tioga, Palo Alto, CA, 1980. unlaing. Motion planning with iner[35] C. O'D tial constraints. Technical Report TR-230, NYU Robotics Laboratory, 1986. [36] J. Pearl. Heuristics. Addison Wesley Publishing, Co., Reading, MA, 1985.
