Real-Time Robot Path Planning via a Distance-Propagating Dynamic System with Obstacle Clearance

Transcription

1 POSTPRINT OF: IEEE TRANS. SYST., MAN, CYBERN., B, 383), 28, Real-Time Robot Path Planning ia a Distance-Propagating Dynamic System with Obstacle Clearance Allan R. Willms, Simon X. Yang Member, IEEE Abstract An efficient grid-based distance-propagating dynamic system is proposed for real-time robot path planning in dynamic enironments which incorporates safety margins arond obstacles sing local penalty fnctions. The path throgh which the robot traels minimizes the sm of the crrent known distance to a target and the cmlatie local penalty fnctions along the path. The algorithm is similar to D bt does not maintain a sorted qee of points to pdate. The reslting gain in comptational speed is offset by the need to pdate all points in trn. Conseqently, in sitations where many obstacles and targets are moing at sbstantial distances from the crrent robot location, this algorithm is more efficient than D. The properties of the algorithm are demonstrated throgh a nmber of simlations. A sfficient condition for captre of a target is proided. Keywords: dynamic system, path planning, safety margins, obstacle clearance, mobile robot, real-time naigation, dynamic enironment, dynamic programming. I. INTRODUCTION In a preios paper [1], we presented a simple yet efficient distance-propagating dynamic system for real-time robot path planning in dynamic enironments. The algorithm is essentially a distance-transform method [2] [5] applied to a flly dynamic enironment. Distance-transform methods sole the shortest path problem by sing a dynamic programming DP) algorithm on a cyclic network [6]. Here we extend the algorithm to incorporate safety margins arond obstacles; robots not only aoid obstacles, bt trael a safe distance arond them. This is achieed by propagating not jst the distance to a target, bt also the distance to an obstacle. The distance to a target is then modified by a penalty fnction based on the distance to an obstacle along the path. Similar to many robot path-planning approaches, the enironment is discretized and represented by a topologically organized map. For the distance-propagating dynamic system, each grid point has only local connections to its neighboring grid points. Neighbors need not be all at the same distance. At each time step, each grid point i qeries its neighbors to determine their information abot distances to targets and obstacles. Distance information ths propagates otward from A. R. Willms is with the Department of Mathematics and Statistics, Uniersity of Gelph, Gelph, Ontario N1G 2W1, Canada. AWillms@ogelph.ca S. X. Yang is with the Adanced Robotics and Intelligent Systems ARIS) Laboratory, School of Engineering, Uniersity of Gelph, Gelph, Ontario, N1G 2W1, Canada. syang@ogelph.ca. target/obstacle locations throgh neighboring grid points. If a target or obstacle moes, a new wae of information spreads ot from the new location. The algorithm preents target distance information from traeling throgh obstacles. The safety margins arond obstacles are compted in a way conceptally eqialent to distance transformation methods [3], [7], [8], with or local pdating of neighbors acting similar to seqential erosions with small strctring elements [8]. Or algorithm howeer allows for dynamic enironments where obstacles and targets are permitted to moe, and there is no limitation on the size of the obstacles or size of the free space. Tre wae front path planners [2], [9], [1] spread information from a sorce otward in waes to all other points on the grid by pdating a grid point s neighbors in the direction of wae propagation and in the order in which the wae arries at the sites. This information may be simply the distance to a target, or a more complicated fnction sch as a penalized distance for safety considerations, or any other qantity that is intended to be minimized. The order of arrial of waes at different points also depends on the information being propagated. For expositional simplicity we consider here the information as simply the distance to a target. The D algorithm [1], [11] which is a modification of A [9]) and its ariants Focsed D [12], D -Lite [13], and E [14]) are tre wae front planners. D determines the correct order of pdating points by sorting its open list according to the crrent distance to the target; pdates for points close to the target occr before those frther away. By sorting its open list, D ensres that all points that hae moed off its open list hae recorded the optimal distance to the target p to the crrent information aailable in the map). The primary featre of D is that it is capable of re-compting new optimal trajectories when alterations to the map are made the moement of an obstacle or target is detected) withot haing to necessarily recompte the entire soltion. Only the soltion down stream from the alteration is re-compted. Ths, for example, if an obstacle half way between the target and the robot shifts its location, only the soltion from this distance ot to the robot needs to be re-compted, howeer, if the target itself moes D essentially needs to re-compte the entire soltion from scratch. D is most efficient when the alterations to the map occr at points close to the robot [1], and this makes it well-sited for a robot path-planning problem where the robot is eqipped with an on-board sensor of limited range, and where information is not being incorporated from other distant

2 2 POSTPRINT OF: IEEE TRANS. SYST., MAN, CYBERN., B, 383), 28, sorces. Or algorithm is similar to D in that soltions are changed down stream from where the map alteration occrred. Howeer, nlike D, or algorithm does not maintain a sorted open list bt rather simply pdates each point in trn. Speed is gained by not haing to maintain a qee bt is lost becase points far from the optimal soltion are also being pdated. Conseqently, in comparison with D, or algorithm is best sited for highly dynamic enironments where alterations to the map are being incorporated from all points, not jst those in proximity to the robot. Or algorithm is not a tre wae front planner since information does not spread to neighboring points in the order of the distance from the target. This means that occasionally the soltion at a particlar point may not be the minimal distance to the target een assming the map is completely p to date. Howeer, gien a constant map, or algorithm does qickly conerge to the optimal soltion [1]. We accept this sboptimality in order to achiee simplicity and efficiency by dispensing with the necessity to determine which point needs to be pdated next. Compting each point in trn rather than working from the target otwards as done by D may make it seem like or algorithm needs to do considerably more work. This is certainly the case for constant enironments or ones where only map alterations associated with obstacle moements close to the robot are made. Howeer, if we are in highly dynamic enironments where many obstacles are moing, where the target itself is moing, and where this information is aailable locally to the points where the moements are occrring, and not jst in a limited proximity to the robot, then or algorithm is actally more efficient. We emphasize that in or algorithm, the comptations at each point se only local information. This means that or algorithm is exceedingly easy to parallelize in a shared memory architectre: the grid points are diided into sbsets and assigned to different processors for pdating. In contrast, most other path-planning algorithms se global information in some way and hence are difficlt to parallelize. Algorithms based on a dynamic programming approach make se of global information by stepping throgh the nodes in an order determined by the crrent recorded distances at each node or the order in which node ales most recently changed. For example, the A algorithm [9] estimates the distance from a node n to the goal as the sm of a locally propagated distance gn) from the starting point to n, pls an estimate hn) of the distance from n to the goal a piece of global information). Een if A is implemented with a non-informed heristic, hn), the algorithm still sorts the nodes in its open list in ascending distance order, which implies the algorithm has global knowledge of the crrent distance for each node. The D family of algorithms also sort based on global knowledge of the crrent distance at each point in the grid, and focsed D also ses global heristics like A. Neral network approaches to robot path planning [15] [19] are similar to or approach in that information abot the location of the target and obstacles is propagated throgh local neighbors, howeer or algorithm has seeral adantages oer these. Typically, in a neral network approach, target locations in the grid inpt a positie actiity to the network and obstacles are either sinks or held at a minimm actiity leel. Actiity is then propagated throgh the network by local connections according to some ordinary differential eqation ODE) model, and robots follow the path of steepest ascent to the target location. Unlike or algorithm where the penalized distance is propagated throgh the network, the actiity ales of most neral networks do not hae a direct physical meaning. In addition, althogh correlated with distance, actiity leels often sffer from satration effects [1] where the gradient in the actiity is ery small and/or shows considerable sensitiity to the arbitrary parameters of the model. Finally, the comptational effort for neral networks is generally considerably more, since nmerically soling the ODE reqires more work than the simple comptations in or discrete algorithm. Or original algorithm is similar to the dynamic wae expansion neral network model proposed by Lebede et al. [2]. Their model does not record the physical distance to the target bt rather the sm of twice the grid distance and the nmber of time steps since the target last moed. Also, their algorithm ses only integer arithmetic which decreases comptation time bt conseqently only gies physically minimal distance paths if all neighbors are an eqal physical distance from each other. Ths, een for reglar sqare grids, neighbors are restricted to the for horizontal and ertical neighbors; diagonal connections are not allowed. In addition, their algorithm as specified redces comptation time by choosing the first neighbor which is passing pdated information. They do this to aoid checking all neighbors. Howeer, as a reslt, their algorithm will generally fail to find the optimal path in sitations where moing obstacles sddenly open p shorter paths to a target. To rectify this sitation, their algorithm wold need to check all neighbors to determine the best information. In contrast, or algorithm records the actal physical penalized) distance to the nearest target. This information may be sefl in a real sitation where the behaior of the robot may wish to be altered depending on the proximity of a target. In addition, or algorithm works for any grid, the only reqirement is that each grid point has a predefined set of neighbors at known distances throgh which the robots and targets may trael. Since we are concerned with changing enironments and moing targets, it is not possible to gie a measre of the comptational effort reqired for a robot to reach a target on a grid of certain size withot reference to the particlar enironment and how it is changing. Indeed, it is easy to constrct changing enironments where the robot can neer reach a target no matter how fast the robot moes [1]). Howeer, the comptational effort for each node in the grid to be pdated once is clearly proportional to the total nmber of points, M, since each node mst simply qery the ales of its finite set of neighbors. See [1] for a comparison of or algorithm with a nmber of others. Althogh or exposition and most of or simlations are in terms of a mobile robot becase this scenario is easy to isalize, the algorithm has more application for robots sch as maniplator arms see Section III-D) where a complete grid of the configration space is more likely attainable.

3 WILLMS & YANG: REAL-TIME ROBOT PATH PLANNING WITH OBSTACLE CLEARANCE 3 II. THE ROBOT PATH ALGORITHM In this section, we present the distance-propagating dynamic system with obstacle clearance, and the algorithm for the robot, target and obstacle moement. For a description of the original algorithm withot obstacle clearance, see [1]. Note that [1] ses different notation than we se here. A. The Penalized Distance-Propagating Dynamic System Sppose the robot enironment is discretized into a grid of M points, labeled by an index, i, each point being either a free space or an obstacle location. The targets and the robot may occpy any free space. For each point i, B i is the set of its neighbors, and d ij is the physical distance from i to neighbor j. B i cold be, for example, the eight nearest points to i in a reglar sqare grid, bt in general, cold be any set of points which yo wish to define as the neighbors of i. Howeer, it is assmed that the targets and robots may only moe from points to neighboring points. We define d min and d max to be the minimm and maximm distances between any two neighbors in the grid. Each grid point has two associated real-aled ariables: x i n) which records the distance to the nearest obstacle at time step n, and y i n) which records the penalized distance to the nearest target. In addition, each point maintains two integer-aled ariables, p x i n) and py i n) which record the parents for x and y, that is, the neighbors of i throgh whom the ales of x i n) and y i n) were calclated respectiely. Define a local obstacle penalty fnction, qx), qx) < q max, where q max is some finite constant. The fnction qx) is the penalty per nit distance traeled. The cost of trael, that is, the penalized distance, along a path that is a distance x from an obstacle is eqialent to an npenalized path that is 1+qx) times longer. The system is initialized as {, if an obstacle is at i,, x i ) = D, otherwise, p x i) = i, {, if a target is at i, y i ) = D, otherwise, p y i ) = i. where D as some large maximal penalized distance. The precise ale of D is not too important. If D is chosen too small, then only grid points which are less than a penalized distance D from a target will participate in the algorithm. A sfficiently large maximal D ale which ensres all grid points participate is gien by D > M 1)d max [1+q max ]). The dynamic system eoles as follows: {, if an obstacle is at i, x i n) = ) 1) min xj n 1)+d ij, otherwise. j B i i, if an obstacle is at i, p x in) = ) argmin xj n 1)+d ij, otherwise. 2) j B i D, if an obstacle is at i, d min qx i n)), if a target is at i, y i n) = [ min y j n 1)+d ij 1+qxi n)) ]) 3), j B i otherwise. i, if an obstacle or target is at i, [ p y i n) = argmin y j n 1)+d ij 1+qxi n)) ]), 4) j B i otherwise. if y i n) D y i n) = D if an obstacle is not at i & qx i n)) > p y i n) = argmaxx j n), 5) j B i else p y i n) = i. where the fnction argmin j Bi fj) retrns the first element j in B i at which fj) is a minimm. The argmax fnction is defined analogosly. In Section II-C we discss how we sort B i so that argmin and argmax preferentially select an appropriate parent when more than one possibility exists. The first step of the eoltion, 1) and 2), simply comptes the distance to the nearest obstacle, keeping track of the x-parent, that is, the neighbor throgh whom the minimal distance is measred. Note that the argmin in 2) is compted at the same time as the minimm in 1). If an obstacle is at i, we define the x-parent of i to be itself. Eqations 3) and 4) compte the penalized distance to the nearest target, keeping track of the y-parent again, the argmin in 4) is compted simltaneosly with the minimm in 3)). In this case, if either an obstacle or a target is at i, we define the y-parent of i to be itself. The significance of setting y i to be nonzero when i is a target is discssed below in Section II-B. Finally, 5) modifies the parent of y i if the distance to the target is as big as D; this is for robot moement and is discssed in Section II-C. Note that the dynamical system, 1) 5), ses ales of xn 1) to pdate xn) and p x n), and ales of xn) and yn 1) to pdate yn) and p y n). Ths xn) mst be compted before yn). It is not necessary to store ales of x, and y for all n, the crrent and preios ales are sfficient. An illstration of how Eqations 1) 4) work is shown in Figre 1. The ariable x i n) records the distance from i to the nearest obstacle at time step n. Of corse obstacles can moe and it takes some time for the entire system to reflect this alteration, so the ale of x i n) may not be p to date for all i. More precisely, x i n) records the minimm oer k {,1,...,n} of the physical distance from i to the nearest obstacle which was k grid steps away from i at k time steps in the past. The physical distance, x, and the nmber of grid steps, k, are related by kd min x kd max. Define f to be the system pdate freqency: the nmber of time steps per nit of real time. Ths if x i n) is less than D, it records information from between xin) d max /f and min n, xin) d min )/f time nits in the past, where w and

4 4 POSTPRINT OF: IEEE TRANS. SYST., MAN, CYBERN., B, 383), 28, [D,1] [D,1] [D,3] [D,3] [D,5] [D,5] [1,2] [6 2 2,3] [ 2,2] [5 2 2,4] n = [,2] [D,2] 1 2 [D,4] [,4] 3 4 [D,6] [D,6] 5 6 n = 2 [,2] [D,2] 1 2 [1,2] [1,4] 3 4 [1,2] [2 2,4] [ 2,2] [4 2 2,4] [D+ 1,3] [ 2,4] [1,2] [ ,4] [ 2,2] [5 2 2,4] n = 1 [,2] [D,2] 1 2 [1,2] [1,4] 3 4 [D+ 1,4] [1,4] 5 6 n = 3 [,2] [D,2] 1 2 [1,2] [1,4] 3 4 lower cost distance pls penalty) than all rotes which stay ot of the safety margin, then the former will be chosen. If hard safety margins were desired then one cold simply define the points in the safety margin to be obstacle points that is, grow the obstacle) and then se the original algorithm withot obstacle clearance. In particlar, the soft margins allow the robot to pass adjacent to obstacles if there is no other safer way to reach a target. The precise manner in which the penalty information has been applied warrants some discssion. Along the path between any two neighbors, the distance to the nearest obstacle is in general changing. In a continos setting, the penalty shold be specified as Q = dij qxs)) ds, 6) where s is the distance along the path. Since xs) is only known at the two end points of this integral, an obios approximation is [1 + 2,3] [5 2 2,3] [2,4] [2,4] 5 6 [1 + 2,3] [1 + 2,4] [2,4] [2,4] 5 6 Fig. 1. Illstration of the dynamic system eoltion, eqations 1) 4). The figre shows for time steps for a nit grid with six points, labeled 1 throgh 6. Point 2 is an obstacle shaded) and point 4 is a target thick circle). The two sets of nmbers inside each circle are [x,p x ] pper set), and [y,p y ] lower set). The initial sitation, n =, is displayed in the pper left. The penalty fnction is qx) = max3 2x,). Atn = 1, points 1 throgh 4 hae correct x ales and x-parents while points 5 and 6 hae not yet receied information abot the obstacle. By step n = 2 all points hae correct [x,p x ] pairs. The sitation is more complicated for the y ales. For example, at n = 1, point 1 has x1) = 1 hence qx1)) = 1 and so y1) is 2 2, while point 3 has x1) = 2 hence qx1)) = and so y1) is Also at n = 1, the y ale for point 4 increases to 1 de to the second line of 3). This increase in y for the target at n = 1 cases alterations in y for its neighbors at n = 2. For example, the comptation of y2) for point 1 yields with the parent being point 3 since the penalized distance to point 4 from point 1 is q1)) = > = q1)). By n = 3 thogh, y ales for all points hae conerged to their appropriate ales. w are the smallest integer greater than or eqal to w and largest integer less than or eqal to w respectiely. Similarly, y i n) records the minimal cost penalized distance to a target) for point i at time step n, which may be ot of date by at yin) d min /f real time nits. most Since the minimization is performed by searching oer the neighbors of each point and each point has a finite nmber of neighbors, the comptational brden for this penalized distance-propagating dynamic system the time reqired to pdate eery grid point once) is proportional to the total nmber of points, M. B. Penalty Fnction The relatie degree of safety is specified by the ser in the penalty fnction formation. The safety margin arond an obstacle is defined as the region in which the penalty fnction is nonzero. The safety margins arond the obstacles are soft in the sense that if a rote that passes into a safety margin is Q d ij 2 qx i)+qx j )). Howeer, in 3) the single end-point approximation Q d ij qx i ) has been sed. The reason for this is that the twopoint approximation yields a dynamical system with approximately 5/3 the nmber of arithmetic operations as the onepoint approximation. If extra storage is sed to hold the ales d ij 1+qx j )+qx i ))/2), then this factor can be redced to abot 4/3. In any eent, we did not feel that the improement in the approximation warranted the slow down in the algorithm. Also to minimize comptations, the penalty at x i is sed in 3) rather than at x j ; ths the one-point approximation for the penalty is applied at the phill ends of the path segments, that is, at the ends closest to the robot. Consider the sitation where a target at point i lies within the safety margin of some obstacle bt the rest of the points along the optimal path to the robot lie otside all safety margins. If y i were zero, then, the distance along this path wold incr no penalty. In order to aoid ignoring a penalty contribtion at the target end of the path, the ale for y i when a target is at i is set to d min qx i ) rather than zero. Howeer, this only has any effect on the algorithm if there is more than one target. The form of the penalty fnction is important. It is reasonable to assme that qx) is a decreasing fnction of x, and that beyond some certain ale, x = B, q is zero althogh sch local limitation on the size of penalty margins is not essential). The simplest sch form for qx), is a piecewise linear fnction { AB x), x < B, qx) = 7), x B, where A > is a strength factor and B > represents the width of the safety margin. Howeer, other forms are possible. The simlations of Section III illstrate that the algorithm can be ery sensitie to the precise penalty fnction being sed. For 7), increasing A has the effect of hardening the safety margin so that the robot is less likely to get close to an obstacle, and increasing B widens the margin.

5 WILLMS & YANG: REAL-TIME ROBOT PATH PLANNING WITH OBSTACLE CLEARANCE 5 C. Robot Moement We assme that the robot can moe from any grid point to any neighboring free space, that is, point robot dynamics. The robot location, rt), is specified as an index of one of the points on the grid, and is a discontinos) fnction of real time t t. Initially, rt ) = i. We assme that the robot s trael path is pdated at a set of real time ales t 1 < t 2 < t 3 <..., and that the robot s actal location for t t k,t k+1 ) is somewhere between the grid points rt k ) and rt k+1 ). At time t k, the next pdate time, t k+1, and next pdate location, rt k+1 ), are determined. The latter is defined as rt k+1 ) = p y rt k ) nrtk ),t k ) ), 8) where ni,t k ) means the highest time step n for which y i has been compted p to time t k. Ths, when a robot arries at location i it proceeds to the crrent y-parent of i. For example, consider the sitation shown in Figre 1 and sppose the points are pdated in their label order and that pdating each point reqires.1 time nits. To pdate all six points once wold reqire.6 time nits. Sppose the robot is at point 5 at time t k = 2. r2.) = 5). For this ale of t k, ni,t k ) is 3 for points i = 1 and i = 2, bt is only 2 for points i = 3,...,6. Conseqently, the next position of the robot will be rt k+1 ) = p y r2.) 2) = 3. Howeer, if t k was 2.3 or higher then n5,t k ) = 3 and the next robot position wold hae been rt k+1 ) = 4. When compting y i n) in 3), it is possible that more than one index j B i attains the minimm of f = y j n 1) + d ij 1 + qxi n)) ). Which index to select as the parent of y i n) is not niqe. The method we sed was to select the neighbor j B i sch that f was minimized and the angle from i to p y i n) was as close as possible to the angle from i to p y i n 1). In other words, we minimized changes oer time in the direction of the optimal path throgh i. To achiee this, we had each grid point, i, store not jst a set B i of its m neighbor indices, bt m differently ordered ersions of B i, each in increasing angle magnitde starting from one of the m neighbors. These orderings are determined by the geometry of the grid and do not change, hence need only be compted once at the beginning. If p y i n 1) is not i that is, if the parent at the preios time step is one of the neighbors) then the neighbor p y i n 1) is always qeried first, followed by the other neighbors in increasing angle magnitde order. The ale of p y i n) only changes if f as measred throgh the next neighbor strictly decreases. It is possible that while a robot is waiting at a grid point with y = D) for distance information to be propagated to it from a target, an obstacle moes toward the robot and wold collide with it if the robot did not moe. Since the dynamical system calclates the distance to obstacles, this information can be sed to make the robot moe away from obstacles when it does not know which way to go toward a target. This is the prpose of 5). If i is a free space with y i n) = D, and the penalty ale qx i n)) is positie, then a frther comptation is performed to set the parent of y i to be the neighbor that is frthest from an obstacle. Again, this maximm may be attained by seeral neighbors so we qery the neighbors in the order described aboe so that again the changes in the angle of the potential robot paths are minimized oer time. Note that the pdate interal, t k = t k+1 t k, need not be constant nor predetermined. For example, with a reglar nit sqare grid, if the robot moes at constant speed r, then t k will be either 1/ r or 2/ r time nits, depending on whether the distance from rt k ) to rt k+1 ) is 1 or 2. In the case of a static enironment, once the location at which the robot resides has settled to its final y ale which occrs when the nmber of time steps exceeds the nmber of grid steps between the target and the robot ia the shortest distance path) eoltion of the dynamic system can cease and the robot can simply follow the minimm distance path to the target sing 8). D. Target and Obstacle Moement Alterations to the enironment map regarding the locations of targets and obstacles can happen at any time. An alteration at point i will begin to be reflected in the dynamical system as soon as point i or any of its neighbors is next pdated. When a point i that was preiosly a free space becomes an obstacle, all paths from i otward increasing distance) are no longer alid. To increase the speed of compting new soltions for these descendant points, j, the ales of y j and p y j are reset to D and j. This is done before the system resmes pdating points in trn. More precisely, if an obstacle newly appears at point i dring time step n, then the fnction ErasePathi) is called where ErasePathi) for each neighbor j of i if p y j n 1) = i y j n 1) = D p y j n 1) = j ErasePathj) end if end for end Since obstacles can moe, collisions with robots or targets are possible. The simlations of the next section were designed so that obstacles neer collided with the target. Except in the forth simlation where points can be simltaneosly obstacles and targets.) The penalty fnction is designed to keep the robots away from the obstacles bt collisions with obstacles are still possible, for example when the only path to a target passes adjacent to a moing obstacle. If sch a collision occrs then the penalty fnction needs to be increased, and/or the robot speed needs to be increased to preent collisions. III. SIMULATION STUDIES In this section we demonstrate the effectieness and efficiency of the proposed algorithm with arios simlations. In all of these simlations the system clock is sed to create a real-time enironment. For example, if the robot begins system clock at t = ) traeling at two nits per second toward a point i that is one nit away, the robot will arrie at t =.5 and the decision as to where the robot will next moe

6 6 POSTPRINT OF: IEEE TRANS. SYST., MAN, CYBERN., B, 383), 28, t = 21.3 s t = 42.6 s t = 68.2 s t = 63.9 s t = 85.2 s Fig. 3. The same simlation as in Fig. 2, bt with A = 1.95 in the penalty fnction rather then 2.. With the lower penalty for being close to obstacles, the robot entres inside the safety margins again of width 1+ 2) a nmber of times while chasing the target, reslting in a shorter oerall path and an earlier captre of the target Fig. 2. Target Chasing. For time snap shots of the enironment are shown. Obstacles are black points with dark grey shading arond them. The target is a bll s eye pattern and its path is a thin solid line; it starts at 5,5) and zig-zags arond the obstacles. The robot is a sqare and its path, starting at 1,1), is shown as a thick solid line. Free spaces are shaded from light to dark, light being smallest ales for the penalized distance, y, to a target. The short lines emanating from the free spaces point to the neighbor throgh whom the information for pdating y was obtained, that is, they point in the direction of greatest decrease in y. The safety margin is 1+ 2 and the robot remains otside these margins ntil t = 71, after which it remains within the safety margin of the third obstacle ntil catching the target at 2,19). is made based on the ale of p y i at time t =.5. Similarly, if an obstacle is moing at a speed of one grid nit per second, then its presence at each sbseqent point on the grid is not signaled ntil the system clock adances another second. A. Target Chasing In this simlation, the target moes at a speed of.35 grid nits per second from the location,) = 5,5) in a zig-zag pattern arond some non-moing obstacles toward 25,25), see Figre 2. The robot starts at 1,1) and traels at a speed of.5 grid nits per second. The penalty fnction is gien by 7) with A = 2 and a safety margin of B = Notice that the robot remains otside the safety margins arond the obstacles ntil t = 71 when it occpies the point,) = 17,14). It enters the safety margin at that point since the target is ery close by at 19,15), and all other safer paths hae larger distance to the target. Thereafter, the robot remains close to the obstacle as it chases down the target, finally reaching it at 2,19). The precise form of the penalty fnction can sbstantially alter the robot s path. For example, if the penalty fnction 7) is altered so that A = 1.95 rather than 2, the reslting robot path is shown in Figre 3. Note that the robot enters the safety margins seeral times becase the penalty for doing so is not as great as remaining otside the margin and traeling a longer path. The robot ends p catching the target earlier at 18,15). Similar reslts can be achieed by decreasing B. 1 5 t = 13.1 s 5 1 t = 26.2 s 5 1 t = 39.2 s 5 1 Fig. 4. Path Abandonment. Three time snap shots of the enironment are shown. Symbols as in Figre 2. The target moes clockwise arond the edge starting in the soth-east corner. The robot, starting in the north-west corner, initially moes east of the central barrier toward the target. Howeer, as the target moes west and the barrier at = 2 slides east, this path to the target becomes increasingly nsafe. Eentally, the robot trns arond and prses the target by going north arond the barrier at a safe distance. This simlation also illstrates how penalty fnctions arond obstacles can preent the robot from reaching a target een if the robot traels considerably faster than the target. If the target contines to weae close in arond obstacles, and the penalty fnction is sfficiently high, the optimal safe path for the robot will be sbstantially longer than the target s path. In the aboe simlation, the robot speed was sfficiently large that eentally the robot caght the target. Howeer, it is not difficlt to imagine a scenario where the speed adantage of the robot is exactly contered by its longer path. Indeed when we re-ran the aboe simlation, either increasing A to 3.5 or B to 3, the robot failed to catch the target ntil after it had halted at 25,25). If the obstacles were arranged in a closed loop and the target woe tightly throgh them, it wold be possible to hae the distance between the target and robot settle into a stable periodic cycle, bonded aboe and below. B. Path Abandonment In this simlation, Fig. 4, the robot s path to the target becomes increasingly nsafe as the target and obstacles moe. Eentally, the robot abandons this path to the target and chooses a safer path. The penalty fnction is again gien by 7) with a safety margin of B = 1 + 2, and A = 2.5. The target starts at,) = 9,1) and moes clockwise arond the exterior at a speed of.25 grid nits per second. There is a stationary north-soth barrier p the center of the grid grid,

7 WILLMS & YANG: REAL-TIME ROBOT PATH PLANNING WITH OBSTACLE CLEARANCE 7 and an east-west barrier that starts by blocking 2 5, = 2. This barrier moes eastward at a rate of.18 grid nits per second ntil it blocks 5 8 at which point it moes back west to its initial placement and so on. The robot starts at 1,9) and moes at a speed of.65 grid nits per second. Initially, the robot moes to the east of the central barrier toward the moing target. Howeer, when the robot reaches 9,3) shortly before t = 19, the path it has been following toward the target now rns adjacent to the east-west barrier along its entire length. The penalty for being this close to the barrier for this many points otweighs the extra distance needed to reach the target by going back north arond the barrier. The robot therefore trns arond and prses the target by going north safely arond the barrier t=3 t=15 C. Sliding Grates This example illstrates how or algorithm is more efficient than D in sitations where the target is moing and there are many obstacles continally moing. Simlations were performed on an Intel Pentim 4, 3.6 GHz machine. Qalitatiely similar reslts were obtained on a faster SGI Altix machine sing larger grids. The D algorithm can determine when a point is recording the p-to-date optimal path to a target. In contrast, or algorithm as we hae specified it here, does not keep track of whether the crrent recorded information at a point is either optimal or p-to-date. As indicated earlier, we accept this sboptimality to maintain simplicity. In most sitations we also expect that the information will not be significantly sboptimal since or system conerges rapidly to the optimal soltion [1], and will not be significantly ot of date proided the system pdate freqency, f, is sfficiently large. Or algorithm cold also be modified to implement a conergence test for optimality as described in [1], or global information cold be tilized to check if the information is p-to-date, if it was felt necessary. In the following simlations, we compare or algorithm with two implementations of D. The first implementation reqires the robot to wait at a point ntil the information there is optimal, and the second allows the robot to moe based on the crrent information aailable. We name these two implementations D -patient and D - eager respectiely. The second implementation allows a fairer comparison with or algorithm. Frther, we hae incorporated safety margins in both implementations of D by modifying the cost fnctions wheneer the distance to an obstacle changes for a point [11]. This distance to an obstacle information is itself compted with another D algorithm which halts when correct distances from obstacles ot to the edge, B, of the safety margin are compted. In the first simlation, a grid of size 89 by 86 is sed. There are two stationary obstacles along the north = 86) and soth = ) bondaries from = 5 to = 85. There are also 11 sets of north-soth grates, at = 5,13,21,...,85. Each set has 12 grates spanning three points and separated by for points. All the grates in a set slide in nison northward ntil the north-most grate toches the north stationary obstacle, then sothward ntil the soth-most grate toches the soth Fig. 5. Sliding Grates. Symbols as in Figre 2. The top panel shows a portion of the grid near the robot s initial location at t = 1 seconds. At this point, the obstacles at = 5 are moing northward, while those at = 13, and 21 are moing sothward. The target is off the graph to the east. The grey line shows a portion of the path of the robot nder or algorithm. Under both D implementations, the robot is still waiting at 1, 1). The bottom panel shows the robot paths nder the two algorithms when the target is captred at the eastern edge. Under D -eager dark line), the target is captred at t = 5 seconds while nder or algorithm grey line) it is captred sooner at t = 23 seconds. Under D -patient the robot neer leaes the starting point. The stars on the two robot paths are at fie second interals, to allow comparison of the two paths in time. Two of these times are labeled. stationary obstacle, etc., at a speed of two nits per second. The initial north-soth location of each grate set is randomly selected. See Figre 5. The target starts in the soth-east corner and moes northward with a speed of two nits per second; if it reaches the north-east corner it begins moing soth at the same speed. The robot starts in the soth-west corner, at,) = 1,1), and moes with a speed of six nits per second. The penalty fnction is gien by 7) with A = 2 and B = 4. Under or algorithm, the robot safely reaches the target at time t = 23. Using D -eager, where the robot is permitted to moe based on sboptimal information, the robot captres the target at t = 5. If the robot is reqired to wait ntil optimal information has been compted D -patient) then the robot neer actally leaes the starting point. The many map alterations de to distant moing obstacles and targets case D to continally insert points onto its sorted open list and to make changes to its path cost fnctions. The time reqired to do these alterations and the sorting slows the propagation of information otward to the robot. Conseqently, nder both D implementations the robot ends p waiting at the starting location for a long time 21.3 seconds) before any information arries there. Under D -eager, once the robot starts to moe the algorithm performs comparably to ors, with the robot captring the target abot 29 seconds later. Under D -patient, een thogh some information arries at 1, 1), the continal pdating of target and obstacle locations oer the whole grid does not allow D to eer complete comptation of optimal

8 Y m) 8 POSTPRINT OF: IEEE TRANS. SYST., MAN, CYBERN., B, 383), 28, m.7 m φ 4 t = 7.5 s t = 15. s t = 22.4 s 1 m X m) θ 3 2 Fig. 6. Maniplator arm specifications. The barrier and target moe left to right at constant height and speed. information at 1,1), and hence the robot neer moes. In comparison, information nder or algorithm first arries at the robot at time t =.6 and the robot immediately begins to moe. We also ran similar simlations with different height grids. Remoing one grate from each set so the grid is 89 79) was enogh to make all three algorithms comparable. Captre times were: t = for ors, t = for D -patient, and t = 23.1 for D -eager. If one grate was added to each set so the grid is 89 93), then nder or algorithm the robot captred the target at t = 23.2 while nder both implementations of D the robot neer moed. This is beyond D s limit of size for this example on or machine; the continal pdates to obstacle and target moements completely occpy the comptations preenting information from eer reaching all the way across the grid to the robot s initial location. Or algorithm was sccessfl for mch larger grids also, for example it captred the target at t = 46 seconds nder a grid of size sets of 2 grates). We also performed similar simlations on a faster compter an Altix 35 with an Itanim 2 processor) and fond qalitatiely similar reslts with larger grid sizes. D. Maniplator Arm In this simlation we consider a two-hinged maniplator arm as shown in Figre 6. A barrier of length 45 cm moes from left to right across the arm s field at a height of 11 cm and a speed of 1 cm per second. At its trailing edge and 25 cm higher, a target moes with the same elocity. The first link of the arm has length 1 m and is initially pointing straight pward θ = 9 degrees). The second link is.7 m and initially the angle between the two arms, φ, is 243 degrees. The task is to make the tip of the arm reach the target. The arm s speed is set at 3 degrees per second, that is, θ 2 + φ 2 = 3. Rather than physical space, the optimal path is determined in the arm s configration space, θ,φ). In this space, the bar shaped barrier becomes an irreglarly shaped obstacle since no part of the arm is allowed to contact the barrier. Also, the mapping from configration space to physical space is two to one except when φ is a mltiple of π), hence the single target has two representations in configration space. Also, since there are maniplator arm configrations which reach the target bt which also intersect the barrier, a point in configration space can be both an obstacle and a target. In this case, to aoid moing toward sch a point, the obstacle takes precedence in 3) so that y will hae a ale D there Fig. 7. Maniplator arm configration space at three times. Target points which are simltaneosly obstacles are indicated as large solid black circles. Initially the target is not in the arm s field bt the barrier moing toward the arm cases, throgh the penalty fnction, the arm to moe ot of the way. By t = 7.5, the target is in the arm s field bt is nreachable de to the barrier s location. The target first becomes reachable at abot t = 15 and the second link of the arm swings completely arond φ = 2π) in order to reach it, which occrs at t = Figre 7 shows for time snap shots of the enironment in configration space. Here we hae discretized the angles so that there are nine degrees between sccessie grid points. That is, = 1 + 2θ/π, θ π, and = 1 + 4φ mod 2π)/2π). The penalty fnction is again gien by 7) with A =.2, and B =.4. The clock starts when the leading edge of the barrier first enters the arm s field. The target does not enter the arm s field ntil abot t = 7.1, and when it does so, it is nreachable since the barrier location precldes arm configrations that wold otherwise reach the target. This remains the case ntil abot t = 14 seconds when the target becomes reachable from the left. Howeer, well before the target is eer isible to the robot, the barrier has moed sfficiently far into the arm s field to hae collided with the arm had the arm not moed. In the first panel of Figre 7 we see that at t = 5.7 the robot arm has already moed p and to the left a short distance. Moement began at abot t = 2.6.) This moement is in response to the obstacle s penalty fnction becoming nonzero at the robot s location een thogh the ale of y is eerywhere still D on the grid. The robot moement algorithm has the robot moing in a direction the greatest distance from an obstacle if the ale of y is D and the penalty fnction is nonzero. Once the robot s location receies the information abot the reachability of the target, it moes, increasing φ throgh 2π so that the maniplator s second link passes oer the first. The moement of the arm in physical space is shown in Figre 8. E. Hall and Rooms In this final simlation there is a hallway connecting for rooms, each of size 6 m by 11 m. The grid is rectanglar bt not niform. In the center of the rooms the grid spacing is 1 m bt in the hallway, arond the edges of each room, and near the doorways the grid spacing is.5 m, as shown in Figre 9. Each room contains one target moing to random points in the room

9 WILLMS & YANG: REAL-TIME ROBOT PATH PLANNING WITH OBSTACLE CLEARANCE 9 t = 2. s t = 9. s t = 16. s Fig. 9. Hall and Rooms. Three time snap shots of the enironment are shown. Symbols as in Figre 2. Moing obstacles are shown as solid black circles and moe p and down the hall randomly isiting two locations in a room. Initially each room contains one target and the robot starts at the soth end of the hall. The preios 15 seconds of target, robot and obstacle paths are shown in each plot as solid lines. At t = 2 the robot has entered the soth-east room and is chasing down the target there. By t = 9 the robot has captred the target in the soth-west room and has retrned to the hall heading to the north-east room. By t = 16 the target in the north-east room has been captred and the robot is entering the north-west room to captre the final target. Y m) Y m) <= t < <= t < X m) 1.6 <= t < <= t < X m) Fig. 8. Maniplator arm physical space showing the arm, barrier and target moement throgh time. at a speed of.25 m/s. In addition to the permanent walls, there are moing obstacles people ) which primarily moe p and down the hallway randomly isiting seeral points in a room before retrning to the hall and moing on. These obstacles moe at a speed of.5 m/s. The robot starts at the soth end of the hall and moes at a speed of.375 m/s. The robot s goal is to captre all for targets while aoiding all obstacles. The penalty fnction is gien by 7) with A = 1 and B = 2 2. Figre 9 shows three snapshots of this simlation. The locations of the robot, targets, and moing obstacles in the 15 seconds prior to the indicated time are shown as solid lines. At t = 2 the robot has entered the sotheast room and is chasing down the first target. By t = 9 it has captred the target in the sothwest room and is retrning to the hall. The robot moes down the center of the hall since the moing obstacles tend to moe along the sides of the hall. At t = 16 the robot has captred the target in the north-east room and sccessflly crossed a ery crowded hallway and entered the north-west room. The final target is captred at t = A ideo ai file) of this simlation is attached. IV. SUFFICIENT CONDITIONS FOR CAPTURE In [1] it was proed in the case where obstacles are static, that the algorithm always reslts in the robot catching a target proided the system pdate freqency, f, the nmber of times the entire grid system is pdated in each nit of real time) satisfies 3 f > ) 9) 1 d min t 1 r where d min is the minimm distance between any two neighboring grid points, and t and r are the target and robot speeds respectiely. The sfficient condition 9) can be extended to the dynamic obstacle sitation with some restrictions on the obstacle moement. In an nrestricted moing obstacle sitation, no matter how fast the robot speed and the system pdate freqency, and no matter how slow the obstacles moe, it is always possible to constrct an enironment where the target cannot be captred [1]. As an obstacle moes or extends) from one grid point to a neighbor which is on the crrent optimal path from the robot to the target, the length of the path that the robot is taking toward the target may increase either becase the optimal path is displaced one grid point or the optimal path is completely occlded and a new optimal path to the target which goes arond the obstacle is fond). If the nmber of times that obstacle moements case increases in the length of the optimal path is finite, then 9) will also be a sfficient condition for captre of the target in the flly dynamic enironment. As noted in Section III-A, the new algorithm with obstacle clearance ia the local penalty fnction, can reslt in sitations

10 1 POSTPRINT OF: IEEE TRANS. SYST., MAN, CYBERN., B, 383), 28, time step, n,t) /f 1/f 1/f 2/f 3/f time ni, t) nj, t) Fig. 1. The time step for all grid points as a fnction of time. The shaded areas are where ni,t) lies for all i. Two particlar cres thick solid and dashed lines) are also plotted. where, despite moing significantly faster than the target, the penalty fnction may force the robot to take a longer safe path and ths neer catch the target. Here we re-derie 9) to show how the penalty fnction is incorporated into the sfficient condition for captre. First consider ni,t k ) as defined in Section II-C, the highest time step for which y i has been compted by time t k. If f is the system pdate freqency, then ni,t) is as shown in Figre 1. As can be seen from this figre, for all t, ni,t) satisfies ni,t+k/f ) = ni,t)+k, k Z 1) ni,t+g/f ) ni,t)+1, < g < 1, 11) nj,t) ni,t)+1, i,j. 12) Consider now a robot moing along an optimal path, leaing from point i at time t k and arriing at a neighbor j at time t k+1 = t k + d ij / r. We hae at departre, since j = p y i ni,tk ) ), y i ni,t k )) = y j ni,t k ) 1)+d ij 1+q x i ni,tk ) ))) y j ni,t k ) 1)+d ij 13) and at arrial, by 1) 12), y j nj,t k+1 )) = y j nj,tk +d ij / r ) ) = y j nj,tk +rem[d ij f / r ]/f + d ij f / r /f ) ) = y j nj,tk +rem[d ij f / r ]/f ) + d ij f / r ) y j nj,tk )+1+ d ij f / r ) y j ni,tk )+2+ d ij f / r ) 14) where rem[w] = w w. Comparing 13) and 14) we see that we mst consider at most 1+2+ d ij f / r pdates of y j. This corresponds to a real time of t = 3+ d ij f / r ) /f, and in this time, the target can hae moed at most a distance t t frther away from the robot. In the worst case, all of this extra distance is alongside obstacles incrring maximm penalty, so that y j cannot hae increased in ale more than t t1+q max ). Here we hae ignored the time taken for the propagation of the signal back from the new target location, which, if inclded wold redce this pper bond. Ths a sfficient condition for y j nj,t k+1 )) to be smaller than y i ni,t k )) is t 3+ dij f / r ) 1+q max )/f < d ij, which, since d ij f / r d ij f / r, will be satisfied if 3 1 < d ij f t 1+q max ) 1 ). r If the target speed is faster than the robot speed or if q max is too large then the term in parantheses aboe will be negatie and conseqently the ineqality cannot be met. This does not mean the robot will fail to reach the target, only that this sfficient condition is not applicable to the sitation. If the term in parantheses is positie, a sfficient condition for y to be strictly decreasing along the robot s path is f > d min t1+q max) r ). 15) Since the grid is finite, y can only take on a finite nmber of ales, hence strict decreasing along the path implies y eentally reaches zero or the aleqx)d min, and ths reaches the target. Comparing 15) with 9) we see that the addition of penalties for being close to obstacles effectiely cases a increase in the target speed by a factor of 1+q max ). This represents the worst case scenario where the target traels throgh a maximm penalty area, directly away from the robot, and the robot is traeling otside the obstacle safety margins. We emphasize that this is jst a sfficient condition, and we wold expect the robot to captre the target in many sitations where this condition is not met, inclding many sitations where y is not strictly decreasing along the robot s path. Indeed, for all of simlations in Section III, this condition is not met since the q max ales are qite large, yet in all cases the robots captre the targets. V. CONCLUSION The distance-propagating dynamic system algorithm for robot path planning in dynamic enironments has been extended to the sitation where the optimal paths are not simply the shortest path from a robot to a target, bt paths which minimize a cost fnction based on both distance to a target and proximity to obstacles. The algorithm works in real time and reqires no prior knowledge of target or obstacle moements. Updating the distance ales at each grid point is done withot any global knowledge, neither of distances at non-neighboring points nor the pdate history of any point. Ths each point cold be implemented as an independent processor with only local connections to its neighbors. This featre makes it easy to