Variable-resolution Velocity Roadmap Generation Considering Safety Constraints for Mobile Robots Jingyu Xiang, Yuichi Tazaki, Tatsuya Suzuki and B. Levedahl Abstract This research develops a new roadmap method for autonomous mobile robots based on variable-resolution partitioning of a continuous state space. Unlike conventional roadmaps, which include position information only, the proposed roadmap also includes velocity information. Each node of the proposed roadmap consists of a fixed position and a range of velocity values, where the velocity ranges are determined by variable-resolution partitioning of the velocity space. An ordered pair of nodes is connected by a directed link if any combination of their velocity values is within the acceptable range of the nodes and produces a trajectory satisfying a set of safety constraints. In this manner, a possible trajectory connecting an arbitrary starting node and destination node is obtained by applying a graph search technique on the proposed roadmap. The proposed method is evaluated through simulations. I. INTRODUCTION In path-planning of mobile robots, roadmap methods are widely used. In roadmap methods, the workspace is abstracted to a set of nodes, which represent positions in space, and a set of links, which represent connections between nodes in the workspace. To avoid collisions, an important condition is that none of the links intersect obstacles. Pathplanning is conducted by applying graph search methods to the roadmap[1]. One of the advantages of roadmap methods is that once a roadmap is generated, the roadmap can be reused for movement to other locations. Some successful roadmap methods applied in the past are: Voronoi diagrams[2], the Visibility roadmap[3], and the Probabilistic Roadmap Method (PRM)[4]. Because the dynamics of a robot is not considered in these methods, there is no guarantee that a robot can follow the path without violating physical and safety constraints. To include these constraints, some methods attempt to remake the map after path-planning[5] or calculate a new trajectory by observing the environment after path-planning[6]. Since path-planning and trajectory planning are separated in these methods, an additional process is needed after path-planning. In kinodynamic motion planning[7], the motion planning of the robot considers the robot dynamics by using a statespace construction of the configuration of the robot using its velocity, and other parameters. Since a straight line in the state-space does not in general give a valid motion, pathplanning in state-space is not as easy as in configuration space[8]. Various kinodynamic motion planning methods, which are effective in motion planning of many systems All authors are with Mechanical Science and Engineering, Nagoya University, Furo-cho, Chikusa-ku, Nagoya, Japan [k kou, tazaki, t suzuki, blaine]@nuem.nagoya-u.ac.jp with complicated dynamics, were proposed in [9], [10], [11]. However, [9] and [10] are on-line motion planning methods and [11] uses a motion planning method for only the current position and a specified destination. For this reason, these methods cannot be used for other start and destination positions. Based on the above background, this research develops a new roadmap generation method for mobile robots. This research considers a 4D state-space that is constructed by combining the 2D configuration space, which is the position of the robot, and the 2D velocity space of the robot. The map that considers only the configuration space or position of the robot is called the configuration roadmap (C-roadmap). The map that considers the state-space representation of the robots position and velocity is called the Variable-resolution Velocity Roadmap (VVR). The C-roadmap can be generated by existing methods, and is extended to VVR by adding velocity information. A node in VVR denotes a region in the state-space that is constructed by combining positions and velocities. Thus, a node in the VVR is a pair of a configuration point and a region of velocity space. If there exists a valid input from any states in one node to any states in another node, an ordered pair of nodes is judged to be a possible transition and the ordered pair of nodes is connected by a directed link. Using this method, the robot can determine whether a path, which is obtained by pathplanning, can also be physically followed. Moreover, because a valid input can be obtained at path-planning phase, online calculation costs are reduced greatly. Furthermore, the proposed method does not need to remake the map for each destinations unlike the potential method. The remainder of this paper is organized as follows: In Section II, an autonomous mobile robot, which is considered in this study, is described. In Section III, the proposed method is explained. In Section IV, the constraint conditions of the mobile robot are explained in detail. In Section V, VVR using the proposed method is shown. Finally, concluding remarks are given in Section VI. Notation: A series {p 0, p 1,..., p n } is written as {p i } 0:n. II. SYSTEM DESCRIPTION In this paper, the mobile robot is assumed to be able to move in all directions in the 2D workspace. Therefore, the 2D configuration space consists of positions in real space, which are represented by p P R 2, and the 2D velocity space which are represented by v V R 2. The state of the robot in the spaces is then denoted by x = [p T, v T ] T X, X = P V.
A. Definition of C-roadmap The workspace is composed of obstacles P obs P and free space P free P. There exists a C-roadmap M = (P, L), which is a connected graph in the workspace with a set of nodes, P, and a set of links, L [P] 2. Let Π[M] = {{p i } 0:n n N, p i P, (p i, p i+1 ) L} be a set of paths in M, and Π[M](p, p ) = {{p i } 0:n Π[M] p 0 = p, p n = p } be a set of paths between two points p and p. As shown in Fig. 1, let d(p, p ) > 0 be the length of a line segment pp, w(p, p ) > 0 be the safety margin for a line segment pp, and ϵ 1 (p, p ) > 0 and ϵ 2 (p, p ) > 0 be the safety margin for the positions p and p. B. Model and Control of a Omni-directional Robot The robot can travel from one configuration position p to another configuration position p. As depicted in Fig. 1(a) and (b), a global coordinate can be defined as (ˆp X, ˆp Y ), and a local coordinate as (p x, p y ), respectively. In the local coordinate system, let p be the origin, the direction of a line segment pp be the x-axis, and the perpendicular direction to the line segment pp be the y-axis. Also let v(v ) be the velocity of the robot at p(p ). As depicted in Fig. 1, the relationship between velocities in the global and local coordinates is a planar rotation in which the angle of rotation is a function of p and p in the global coordinate. Since the robot can move in all directions in the 2D workspace, the dynamics of the robot is given by: p(t) = u(t), (1) where p(t) = [x(t), y(t)] T is the position of the robot at time t, u(t) = [u x (t), u y (t)] T is the acceleration input at t. A acceleration profile that drives the robot between two positions can be defined by constant acceleration along the x-axis and two separate steps of constant acceleration along the y-axis as in: u x (t) = c x, t [0, T ), { u y c y1, if t [0, T/2), (t) = c y2, otherwise, where c x, c y1, c y2 are constant values, and T R + is the traveling time between the two positions. To constrain the path, the following boundary conditions can be specified: p(0) = (0, 0), p(t ) = (d(p, p ), 0), (2) v(0) = v, v(t ) = v, (3) where v(t) = [v x (t), v y (t)] T is the velocity of the robot at t. From the boundary conditions, c x, c y1, c y2 and T are obtained as shown below: c x = ( v x + v x )/T, (4a) c y1 = (3v y + v y )/T, (4b) c y2 = (v y + 3v y )/T, (4c) T = 2d(p, p )/(v x + v x ). (4d) where (v x, v y ) is the velocity in the local coordinate. This means that u(t) and T are uniquely identified when p, v, (a) Global coordinate Obstacle Obstacle (b) Local coordinate Fig. 1 Definition of information between two points p and v are defined, and p(t) is uniquely identified when u(t) is defined. Finally, define this trajectory the function as: (u, p, T ) = F (p, v, p, v ). C. Constraint Condition for Movement For the movement of the robot, two constraint conditions are considered: a safety constraint, which is for collision avoidance during movement, and a specification constraint, which can arise from the physical limitations of the robot. As shown in Fig. 1(b), there are two safety constraint conditions given to the robot. In the first, to avoid obstacles, the robot should not leave the line of movement by more than w(p, p ). In the second, the robot should not depart from both ends of the line segment more than ϵ 1 (p, p ) and ϵ 2 (p, p ), respectively. This gives the following constraint conditions: y(t) w(p, p ), p x ϵ 1 (p, p ) x(t) p x + ϵ 2 (p, p ), t [0, T ], (5a) t [0, T ]. (5b) As an example of a specification constraint, consider the fact the all robots have an allowable upper limit of velocity and acceleration. Let V max be the upper limit of velocity and A max be the upper limit of acceleration. The specification constraint for the robot is then: v(t) V max, t [0, T ], (6a) u(t) A max, t [0, T ]. (6b) From the safety and specification constraints, a trajectory can be uniquely identified when the positions and velocities of the starting and ending points are defined. We can define the feasibility and a feasible region for a pair of starting and ending points as: Definition 1 (Feasibility) A pair of (p, v) and (p, v ) is feasible if and only if the trajectory (u, p, T ) = F (p, v, p, v ) between the pair satisfies the constraint conditions (5) and (6). A series {(p 0, v 0 ), (p 1, v 1 ),..., (p n, v n )} is feasible if and only if (p i, v i ), (p i+1, v i+1 ), (i = 0, 1,..., n 1) is feasible.
Feasible region Feasible region (a) Illustration of a configuration roadmap (a) A convex feasible region (b) A non-convex feasible region Fig. 3 Examples of feasible region (b) Illustration of augmentation of velocity information (c) Illustration of augmentation of velocity information in finely Fig. 2 Illustration of extension a configuration roadmap to a state roadmap Definition 2 (Feasible Region) Given p and p, the set of (v, v ) such that the pair of (p, v) and (p, v ) is feasible, is called the Feasible Region, F(p, p ). F(p, p ) = means that a transition from p to p is impossible, using the proposed controller. III. STATE ROADMAP GENERATION METHOD In this section, we explain the proposed method in detail. Figure 2(a) shows a possible path from p 1 to p 4. The robot should select a valid velocity at each position to track the path. If a velocity sequence is not valid, the robot is unable to reach p 4 from p 1 as depicted by the red dotted lines in Fig. 2(a). To determine if a velocity sequence will be valid, the velocity information can be added to Fig. 2(a), as shown in Fig. 2(b) and 2(c). Since the velocity space is considered as 1D, Fig. 2 is drawn in 3D. A. Augmentation of Velocity Information This section discusses how to extend the C-roadmap, M = (P, L), to a VVR using the velocity information. The VVR can be expressed as M v = (X v, L v ), where X v is the set of nodes and x = (p, v) X v is a pair of a position p P and a velocity region v V, and L v [X v ] 2 is the set of directed links. Let V(p) = { v (p, v) X v } be a set of velocity regions at p, Π[M v ] = {{(p i, v i )} 0:n n N, x i X v, (x i, x i+1 ) L v } be a set of paths in M v, and Π[M v ](p, p ) = {{x i } 0:n Π[M v ] p 0 = p, p n = p } be a set of paths from p to p in M v. A definition of the connectivity of the nodes is given below: Definition 3 (Connectivity of State Nodes) The following relation holds for x = (p, v) and x = (p, v ) in M v = (X v, L v ): (x, x ) L v (p, p ) L and ( v, v ) F(p, p ). This means, if (p, v), (p, v ) for v v, v v is feasible, a directed link from x to x is constructed. From this definition, we have the following lemma: Lemma 1 For an arbitrary path {x i } 0:n Π[M v ], x i = (p i, v i ) in M v, a series {(p 0, v 0 ), (p 1, v 1 ),..., (p n, v n )}, (v i v i, i = 0, 1,..., n 1) is feasible. That is to say that an arbitrary path in M v gives a feasible trajectory. B. Checking feasibility conditions This section discusses how to check whether v v F(p, p ). As shown in Fig. 3, because F(p, p ) is not in general a convex region, it is difficult to check whether v v F(p, p ). However, a closed convex region F(p, p ) F(p, p ), which can be chosen freely, can be introduced. Notice that v v is a convex hypercell, since v is rectangular. Let V( v, v ) be the set of vertices of the v v rectangle. For these discussion, we have the following lemma: Lemma 2 Let V( v, v ) be the set of vertices of the v v. V( v, v ) F(p, p ) v v F(p, p ). Based on Lemma 2, for each hypercell, only whether all of vertices of the hypercell v v are in F(p, p ) needs to be checked. Moreover, the constraint conditions only need to be checked 16 times for a pair of x and x. C. Dividing Velocity Space in Variable Resolutions The connectivity of the state nodes greatly depends on the resolution of the velocity space. For example, if velocity space partitioning does not give enough resolution, there exists no safe path from p 1 to p 4 (Fig. 2(b)). In Fig. 2(c), on the other hand, the velocity space is divided with a
Algorithm 1 generate links(p, p ) 1: for all ( v, v ) V(p) V(p ) do 2: V( v, v ) set of vertices of v v 3: if V( v, v ) F(p, p ) then 4: make( directed link between ) (p, v) and (p, v ) 5: else if V( v, v ) F(p, p ) then 6: if v is a leaf of quad-tree, then 7: C t = C t { v} 8: end if 9: if v is a leaf of quad-tree, then 10: C h = C h { v } 11: end if 12: end if 13: end for Fig. 4 An example of variable-resolution partitioning of velocity space Fig. 5 Flowchart of overall procedure high enough resolution that there exists a safe path from p 1 to p 4. In this approach, however, since there are many regions made by this partitioning, the search tree will become large and make path-planning computationally expensive. In general, there is no a prior knowledge about the appropriate resolution. This paper proposes a partitioning method based on variable-resolution. In the proposed method, the variable resolution partitioning of the velocity space is expressed by a quad-tree. One example of partitioning at p P is shown in Fig. 4. Notice that the velocity space partitioning is performed in the global coordinate. A flow of this partitioning process is shown in Fig. 5. Initially, the velocity space at each p is divided into quarters (Resolution 1). Next, generate links(p, p ) shown in Algorithm 1 is performed on all (p, p ) L. In Algorithm 1, C t and C h are the sets that contain the velocity regions. Lines 3-12 of Algorithm 1, determine if each velocity region should be further divided. If the velocity regions are to be divided, v V(p) is stored in C t, or v V(p) is stored in C h. The criterion for dividing a velocity region is based on the relationship between vertices in the velocity region V( v, v ) and F( v, v ) as shown in Fig. 4(b): line 3: Because a directed link is made from (p, v) to (p, v ), v and v should not be divided. line 5: Because a part of V( v, v ) is included in F(p, p ), v and v may be better to be divided. other : Because no part of V( v, v ) is included in F(p, p ), v and v should not be divided. Based on this process, the region which does not need to be divided is not divided. After Algorithm 1 is performed, the termination condition is checked. If the termination condition is fulfilled, map generation is finished. If not, the velocity regions in C t C h are actually divided. IV. APPLICATION TO A ROBOT In this section, the proposed method is applied to the robot, which was presented in Section II, using the variables that were introduced in Fig. 1 and Section III-B. Finding the constraint conditions of the robot is separated into 4 parts, which are traveling time, position, velocity and acceleration. 1) Traveling time: Since T should be positive, from the fourth equation of (4), the following is obtained: v x + v x > 0. (7) 2) Position: Since the full derivation of the y-axis position constraint (5a) is rather time consuming, a general outline is explained using Fig. 6. The trajectories of the y-axis of the robot are classified into two types. The first type has a maximum trajectory at t α, 0 t α T/2, and the second type has a maximum trajectory at t β, T/2 t β T. Notice that because there is symmetry to a y-axis, only 0 v y is illustrated in Fig. 6. From these trajectories, the following is obtained. { y(t α ), if v y v y v y, y max = y(t β ), otherwise, y(t α ) = (v y ) 2 d(p, p )/ {(v x + v x )(3v y + v y )}, y(t β ) = (v y ) 2 d(p, p )/ {(v x + v x )(v y + 3v y )}.(8)
Fig. 6 A region about constraint condition of y-axis Fig. 8 Acceleration region satisfying condition (14): the hatched rectangular region depict F(p, p ). Fig. 7 A region about constraint condition of x-axis where y max is the maximum distance from the link. From this equation, the regions shown in Fig. 6 are obtained. It can be seen, by use of the Hessian matrix, that a union of the three regions is a convex region. From the second equation of (5b), which is the x-axis constraint condition, the following is obtained: don t care, if v x > 0, v x > 0, d(p, p ) + ϵ 2 (p, p )v x ϵ 2 (p, p )v x, if v x > 0, v x 0, d(p, p ) + ϵ 1 (p, p )v x (9) ϵ 1 (p, p )v x, if v x 0, v x > 0. A region which is defined by (9) is also convex (Fig. 7). 3) Velocity: Since the robot performs two steps of uniform acceleration, the velocity in (6a) will be a maximum at one of the three conditions, t = 0, T 2, T. This gives the following: (v x ) 2 + (v y ) 2 V 2 max, (10a) (v x + c x T 2 )2 + (v y + c y1 T 2 )2 V 2 max, (10b) (v x ) 2 + (v y ) 2 V 2 max. (10c) A region defined by (10a) and (10c) is convex, since it is bounded by a circle. Using (4) for (10b), the following is obtained: 0 < (v x + c x T/2) 2 + (v y + c y1 T/2) 2 { } 2 { 2 = (v x + v x )/2 + ( v y + v )/2} y V 2 max. (11) This region is also convex as can be shown by its Hessian matrix. Fig. 9 Acceleration region satisfying condition (16): the hatched rectangular region depict F(p, p ) 4) Acceleration: For the acceleration condition, the acceleration from (6b) is separated into its x-axis and y-axis components: a x A max /2, a y A max /2. (12) Considering a x, using (4) the following is obtained: 2d(p, p )A max (v x ) 2 (v x ) 2 2d(p, p )A max. (13) From Fig. 8, it can be seen that this region is not convex. A region located in this region and is convex, however, can be chosen: 2d(p, p )A max v x 2d(p, p )A max, 2d(p, p )A max v x 2d(p, p )A max. (14) However, optimization is not considerd in this paper. In considering a y, using (4) as same as a x, the following is obtained: c y1 = (3vy + v y )(v x + v x ) 2d(p, p A max. (15) ) 2 Since the region is again not convex as shown in Fig. 9, a representative convex region must be chosen: (3v y + v y )/4 (v x + v x )/2 + d(p, p )A max / 2, (3v y + v y )/4 (v x + v x )/2 d(p, p )A max / 2. (16)
c y2 can be processed similarly: (v y + 3v y )/4 (v x + v x )/2 + d(p, p )A max / 2, (v y + 3v y )/4 (v x + v x )/2 d(p, p )A max / 2. (17) V. NUMERICAL SIMULATION Considering the roadmap generation method described in Section IV, the experimental result shown in Fig. 10 can be obtained. Figure 10(a) depicts a workspace and a C- roadmap, Fig. 10(b) depicts a VVR for the conventional roadmap, and Fig. 10(c) depicts two results of path-planning. In Fig. 10(a), the black circles depict each position, the line segments depict links, and the green areas depict obstacles. In Fig. 10(b), the red circles depict nodes of the C-roadmap, and the rectangles depict the velocity regions of each position. In this experiment, V max = 200, A max = 70, the velocity ranges are 200 ˆv X 200, 200 ˆv Y 200, and the resolution of the VVR is 5. In Fig. 10(c), the black arrows show the velocity of the robot at each position, the blue curves denote trajectories, and the green squares show the selected velocity region at each position. Since the velocity should be 0 when the robot starts and ends movement, regions which include (ˆv X, ˆv Y ) = (0, 0) are chosen at the start and destination. In the proposed method, since acceleration is obtained from (4), the trajectory can be determined. VI. CONCLUSION In this paper, a state roadmap generation method based on variable-resolution for an autonomous mobile robot has been presented. Since the map generated by the proposed method includes velocity information of the mobile robot, inputs to the robot, which consider safety and the physical limitations of the robot, can be found simultaneously in the path-planning phase. The method has been tested through numerical simulation and an example was presented. In the future, the algorithm will be tested through experiments in a real environment, and extended to multi-robot systems. REFERENCES [1] M. Sung, L. Kovar and M. Gleicher: Fast and Accurate Goal-directed Motion Synthesis for Crowds, in Eurographics/ACM SIGGRAPH Symposium on Computer Animation, pp.291-300, 2005 [2] P. Bhattacharya and M. Gavrilova: Roadmap-based path planning - using the voronoi diagram for a clearance-based shortest path, in Robotics and Automation Magazine, Vol.15, No.2, pp.58-66, 2008. [3] D. Avis and G. Toussaint: An optimal algorithm for determining the visibility of a polygon from an edge, in IEEE Trans. Comput., Vol.30, No.12, pp.910-914, 1981. [4] L. E. Kavraki, P. Svestka, J.-C. Latombe and M. H. Overmars: Probabilistic roadmaps for path planning in high-dimensional configuration spaces. in IEEE Trans. on Robotics and Automation, Vol.12, No.4, pp.566-580, 1996. [5] M. Yamamoto, M. Iwamura and A. Mohri; Quasi-time-optimal motion planning of mobile platforms in the presence of obstacles, in Proc. of IEEE Int. Conf. on Robotics and Automation vol. 4, pp. 2958-2963. 2002, [6] M. Kazemi, M. Mehrandezh and K. Gupta: An Incremental Harmonic Function-based Probabilistic Roadmap Approach to Robot Path Planning, in Proc. of IEEE Int. Conf. on Robotics and Automation, Barcelona, Spain, pp.2136-2141, 2005. (a) A simulation environment : Position (b) A variable-resolution velocity roadmap (c) Trajectories: from (0,0) and (1500,0) to (0,1200) Fig. 10 An environment for experiment and a VVR constructed by the proposed method [7] B. Donald, P. Xavier, J. Canny and J. Reif: Kinodynamic motion planning, in Journal of the ACM, Vol.40, pp.1048-1066, 1993. [8] S. M. LaValle: Planning Algorithms, Cambridge University Press, New York, 2006. [9] I. A. Sucan and L. E. Kavraki: Kinodynamic Motion Planning by Interior-Exterior Cell Exploration, in Intl. Workshop on the Algorithmic Foundations of Robotics, Guanajuato, Mexico, December 2008. [10] R. Cowlagi and P. Tsiotras: Multi-resolution H-Cost Motion Planning: A New Framework for Hierarchical Motion Planning for Autonomous Mobile Vehicles, in 49 t h Proc. IEEE/RSJ Int. Conf. on InDecision and Control (CDC), San Francisco, CA, Sept. 25-30, pp. 3501?3506, 2011 [11] S. Karaman and E. Frazzoli: Optimal Kinodynamic Motion Planning using Incremental Sampling-based Methods, in Proc. IEEE Conf. on Decision and Control (CDC), Atlanta, USA, December 2010.