Improving the Performance of Sampling-Based Planners by Using a Symmetry-Exploiting Gap Reduction Algorithm

Transcription

1 Improving the Performance of Sampling-Based Planners by Using a Symmetry-Exploiting Gap Reduction Algorithm Peng Cheng Emilio Frazzoli Steven M. LaValle University of Illinois Urbana, IL USA {pcheng1, frazzoli, lavalle}@uiuc.edu Abstract Although sampling-based planning algorithms have been extensively used to approximately solve motion planning problems with differential constraints, gaps usually appear in their solution trajectories due to various factors. Higher precision may be requested, but as we show in this paper, this dramatically increases the computational cost. In practice, this could mean that a solution will not be found in a reasonable amount of time. In this paper, we substantially improve the performance of an RRT-based algorithm by planning low precision solutions, and then refining their quality by employing a recent gap reduction technique that exploits group symmetries of the system to avoid costly numerical integrations. This technique also allows PRMs to be extended to problems with differential constraints, even when no high-quality steering method exists. I. INTRODUCTION Motion planning problems with differential constraints include both kinodynamic and nonholonomic motion planning problems, in which constraints exist for both the configuration and its time derivatives. The objective is to design an open-loop control history which will safely drive the robot system from an initial state to a goal state. A challenging example is shown in Fig. 1, in which a control history is designed to maneuver a spacecraft close to a large space structure, after the failure of some of the on-board thrusters. Because of pointing constraints on the communication antenna, on-board telescope and star tracker, the spacecraft is under restrictive constraints both on its position and its attitude. There exists strong evidence [4], [5] that motion planning with differential constraints is intractable. Therefore, many approximate methods have been designed. A lot of recent planners are sampling-based planners which include many incremental search algorithms (ISAs) (e.g., RRTs [21]) and roadmap methods (e.g., PRMs [31]). Since dynamics constraints are considered in the planning process and the returned solutions are executable, a lot of incremental search algorithms (ISAs) have been designed recently [2], [3], [9], [11], [16], [22], [28], [23]. The majority of robot systems involve continuous time and inputs; therefore, most ISAs first discretize the space of piecewise continuous control histories into a finite subset, which decreases the problem complexity. A directed search graph is incrementally built, Fig. 1. This extremely challenging problem requires trajectory design for a crippled spacecraft that has complicated orientation constraints (details are in Section VI) and must drift among obstacles. which corresponds to family of partial solution trajectories. Initially, there are a small number of nodes. In each iteration, a node in the graph is chosen and extended to generate new nodes and edges. If a path in the graph that connects the initial state and a goal state exists, then a solution is returned. PRMs have also been used for systems with differential constraints [1], [31], in which either an analytical steering method or an ISA is used as the local planner. One common problem with sampling-based planning algorithms is that there exist gaps in their solutions since the solution control history might not exactly drive the system between two given states. One main reason is the input space discretization. Under the discretization, some systems are not small-time locally controllable (STLC): the sets of reachable states have the structure of a lattice, which prevents the exact matching of the trajectory endpoint with the goal state in a finite number of steps. In cases in which the set of reachable states is everywhere

2 dense [24], or even continuous [10], it is possible in principle to add a sequence of controls to move the final point arbitrarily close to the end goal, but this is done at the expense of the efficiency of the trajectory. Gaps greatly degrade the quality of the solutions, especially when a gap appears far from the end because a small offset early in the trajectory might greatly change the final state after integration. Many ISAs can quickly return low precision solutions with big gaps. However, finding high precision solutions with small gaps usually increases the running time dramatically. If a separate algorithm could efficiently reduce gaps, then ISAs can find high precision solutions faster by first finding low precision solutions and converting them into high precision solutions. One way to reduce gaps is to use steering algorithms [3], [26], [18], [20], [29]. Steering methods are only available for few classes of systems, such as kinematically controllable systems [3] and differentially flat systems [29]. Furthermore, obstacle constraints are difficult to incorporate explicitly in steering methods, and the cost of the resulting trajectories might be dramatically increased at the gaps. Instead of steering, we have developed a method [6] that exploits geometric properties of dynamical systems to efficiently reduce gaps by perturbing the control history. Besides our work, avoiding obstacles by optimizing computed paths for nonholonomic systems is presented in [19], and [15] provides a heuristic method to improve the quality of the solution by searching a smaller space with a higher resolution discretization of the control space. In this paper, we design new planners by combining an RC-RRT [7] with the gap reduction method, and use the new planner to solve several challenging motion planning problems. Also, we show how the gap reduction idea can be used to construct a PRM [14] when a steering method is not available. II. THE DISCRETIZED MOTION PLANNING PROBLEM In this section, we first define the original motion planning problem we wish to solve, and then define a discretized version, used for sampling based planners. Motion planning with differential constraints: A motion planning problem with differential constraints, denoted as P, is a tuple, (X, ρ, D, x init, X goal, U, f). The state space, X, is a compact differentiable manifold of dimension n with a metric function ρ. The constraint function D is a map from X to R and D(x) 0 means that the state x X is violation free. The initial state is x init, and the set of goal states is X goal. The input space U R m is compact, and m n. The dynamics of the system, assumed to be time-invariant, are described by a set of Ordinary Differential Equations (ODEs) of the form ẋ(t) = f(x(t), u(t)), (1) in which x(t) X, u(t) U, and ẋ(t) denotes the time derivatives of the state. Consider a piecewise continuous open-loop control history π : [0, t f ) U. Let Φ π : X [0, t f ) X denote the state transition map for (1) under the action of π, i.e., Φ π (x 0, t), with t [0, t f ), is the solution of the initial condition problem ẋ(t) = f(x(t), π(t)), with x(0) = x 0. Clearly, Φ π (x 0, 0) = x 0. Discretized motion planning problems: Most sampling based planners restrict the search for a solution to the motion planning problem by discretizing time and the control inputs. In other words, given a sampling interval t, and a finite set Ũ U, solutions are parameterized by a finite array of control values π = ( π 0,..., π k 1 ) Ũ k, for some k N, in the sense that π(t) = π t/ t. This transforms P into a discretized motion planning problem P = (X, ρ, D, x init, X goal, Ũ, t, f). We say that the policy π or its discrete parameterization π is an exact solution to P if the following conditions are satisfied: 1) D(Φ π (x init, t)) 0, t [0, t f ), and 2) t goal [0, t f ) : Φ π (x init, t goal ) X goal. Since (finite-length) exact solutions do not exist in general for the discretized problem, approximate solutions must be considered in that case. We say that the control policy π is a solution with tolerance ɛ π if: 1) D(Φ π (x init, t)) 0, t [0, t f ), and 2) t goal [0, t f ), x goal X goal : ρ(φ π (x init, t goal ), x goal ) < ɛ π. In the remainder of the paper, we will often use the shorthand notation Φ t π(x 0 ) := Φ π (x 0, t). III. SAMPLING-BASED PLANNERS AND GAPS We will first analyze the ISAs to see how gaps are generated. Then gaps in roadmap based planners are discussed. Incremental search algorithms: The general form of ISAs is shown in Fig. 2. An ISA will take P and ɛ s as parameters and iteratively build a search graph for the solution, in which ɛ s is the gap tolerance. To obtain a solution with tolerance ɛ π, ɛ s is normally much smaller than ɛ π (A quantitative relationship between ɛ s and ɛ π is conservatively estimated in [8]). The search graph is a directed graph G N, E, in which each node n in N is associated with a state x(n) X, and each edge e E is associated with an input u(e) Ũ. In Line 1, the search graph G is initialized with any starting states. In a single-directional search method, G initially contains a single node, n init, associated to x init. For a bidirectional search method, G also contains n goal, associated to x goal. Similarly, more initial nodes could also be added. Line 3 selects a node, n cur N, and determines the input set U cur Ũ for extension. Line 4 selects a state, x int that may serve as an intermediate goal. A local planner chooses from U cur an input, u new, which drives the system from x(n cur ) toward x int over a time interval t, generating the new state x new. Line 6 determines whether the new trajectory portion satisfies the constraints. If so, then a new node with state x new and a new edge with input u new are added to G. In Line 9, whether a solution exists will be checked against x new. If ρ(x(n new ), x(n)) < ɛ s for some n N, then n new is identified with n in G. Furthermore, if there is a directed path in G passing n new from n init to n goal, then the control sequence in the path will be returned as a solution. Otherwise, the process is iterated, until a solution is found or a termination condition is met, at which point the ISA has failed to find a solution. This might occur, for example, after a specified number of iterations.

3 INCREMENTAL SEARCH( P, ɛ s) 1 E ; N some starting states; 2 repeat 3 n cur, U cur SelectCurrentNode(G); 4 x int SelectIntermediateGoalState(G); 5 u new, x new LocalP lan(x(n cur), x int, U cur, t); 6 if u new leads to a violation-free trajectory then 7 G.InsertNode(x new); 8 G.InsertDirectedEdge(n cur, n new, u new); 9 SolutionCheck(n new, P, G, ɛ s) 10 until TerminationCondition(G) 11 return Failure; Fig. 2. A general template for incremental search algorithms. Gaps in the solutions: For an edge e E connecting n 1 and n 2 in N, we say that there exists a gap in e if Φ t u(e) (x(n 1)) x(n 2 ), in which u(e) is the control associated with the edge. When a solution of P contains a policy which is associated with an edge with a gap, we say that a solution has a gap. Gaps could be induced by many reasons, such as state space discretization, control space discretization, local planners, and numerical integrations. If the state space is discretized into small voxels [2], all states in one voxel will be represented by only one state such that gaps are induced between these states. Gaps also happen when an edge is associated with a control which is designed by a non-exact local planner and does not exactly connect two states. Similarly, numerical integration causes gaps since its results are only an approximation of the actual value. To be concise, only gaps induced by the gap tolerance at Line 9 are considered in the paper. However, methods for this type of gaps could easily be extended to other types of gaps. ISAs normally are single- or bi-directional tree based search. There are no gaps in edges in the trees because one end state of an edge is obtained by integrating the control associated the edge from the other end state. The only gap happens when we check the existence of a solution with ɛ s. Take a single directional ISA for example, the gap of a solution π only exists between Φ t f π and some x goal in X goal. Bidirectional ISAs will have only one gap in the middle of a solution. Roadmap based planners: For roadmap based methods, the roadmap is built by using the local planner to connect states associated with milestones. If only approximate local planners are available, then the gap tolerance ɛ s is used to check the connectivity of those states, i.e., if a control u : [0, t u ) U is designed by an approximate local planner to connect state x 1 and x 2 and ρ(φ tu u (x 1 ), x 2 ) < ɛ s, then x 1 is considered to be connected to x 2 by u. In other words, a gap might be induced in each edge in the roadmap. There could exist more than one gaps in a solution when the solution path passes through several edges in the roadmap. Generally, bidirectional and roadmap based methods are much more efficient than single directional methods since they have less search depth and more chance to detect solutions. However, especially for roadmap based planners, since only few simple robotic systems (e.g., the Reeds-Shepp car [27]) have analytical steering algorithms to exactly connect two states, multiple gaps in the roadmap could seriously affect the precision of the returned solutions. This might be one of the main difficulties associated with extending roadmap planners to motion planning with differential constraints. IV. GAP REDUCTION ALGORITHMS In this section, we will review the main ideas of the gap reduction algorithms in [6]. First, the geometric properties of a class of robotic systems, which are the key to the efficiency of our algorithms, are presented. Following this, we provide the gap reduction algorithms. Symmetries in systems: Consider a Lie group G, with identity element e, acting on the state of the system through the (left) action Ψ : G X X; we will often use the shorthand Ψ g (x) := Ψ(g, x). We call G a symmetry group for the system (1) if the system s dynamics are invariant with respect to the action of G. Invariance is equivalent to the following statement. Given any trajectory t (x(t), u(t)) X U which is a solution to Equation (1), the trajectory t (Ψ(g, x(t)), u(t)) is also a solution to equation 1 for all g G. It can be verified that invariance implies that group actions commute with state transitions. If x f = Φ t f u (x init ), then Ψ g (x f ) = Φ t f u (Ψ g (x init )), i.e., Ψ g Φ t f u = Φ t f u Ψ g. For many robot systems with symmetries, X can be partitioned, at least locally, into the Cartesian product of two manifolds X = G Z. For simplicity of exposition, we will assume that Z = R nz, n z < n. Accordingly, we write the generic point x X as the pair (g, z) G Z. Following conventions from differential geometry, Z is the base space, G is the fiber, and their product X is a principal fiber bundle; see [17]. Gap reduction algorithms: Consider a policy u : [0, t u ) U obtained from a discrete policy π returned from an ISA. Assume that there is a gap between Φ tu u (x init ) and x g, a gap elimination algorithm aims at perturbing u into a new policy v : [0, t v ) U such that D(Φ t v(x init )) 0 for all t [0, t v ) and ρ(φ tv v (x init ), x g ) ρ(φ tu u (x init ), x g ). In principle, the gap reduction problem can be formulated and solved as nonlinear programming problem with objective function ρ(φ tu u (x init ), x g ). Such an approach is however unpractical for nonlinear systems, since the number of degrees of freedom in the choice of the control perturbation v is very large, and computing the effect of a perturbation applied at time t [0, t f ) requires an ex-novo integration of the ODE (1) over [t, t f ). In other words, since the final state x f = Φ tu u (x init ) = Φ t π k 1 Φ t π 1 Φ t π 0 (x init ) and each state transition generally corresponds to an expensive numerical integration, perturbing u i normally requires to recalculate state transitions for { π j } j=i,,k 1 which considerably affects the running time of the optimization. The idea behind our approach to gap elimination is the following. Suppose that it is possible to perturb the control input π i into v : [0, t v ) U, in such a way that Φ tv v = Ψ g Φ t π i for some g G. Then, since the group action Ψ and the state flow Φ commute, the new final state x f = Ψ g x f, which intuitively means that the

4 remaining part of the trajectory can be rigidly translated, without need for re-integration. If it is possible to find a class of perturbations on the control input that satisfy the stated condition, then the final state of the trajectory can be written as an algebraic function of the perturbation parameters. In [6], we identified gap-reduction techniques applicable to systems with feedback-linearizable base dynamics, and with affine in control based dynamics. In this paper, we will consider only one technique, which according to simulation experiments, reported in the same paper, yielded by far the best results. Consider, for simplicity, a system on a principal fiber bundle, whose dynamics are expressed as ġ(t) = g(t)ξ(z(t)), and ż(t) = f z (z) + g z (z)u. If at time t, there exists ū U such that f z (z( t)) + g z (z( t))ū = 0, (2) then a trajectory segment can be inserted after t, over which the base variables remain constant, and the fiber variable evolves according to ġ(t) = g(t)ξ(z( t)). Given any t ( t, t f ), the generic point in the original trajectory can be written as Φ t u(x 0 ) = Φu t t 2 Φ t u1 (x 0 ), where u 1 and u 2 are the restrictions of the control input history u to [0, t), and [ t, t f ), respectively. Define the perturbed control input v τ : [0, t f + τ) U as follows: u(t), if t < t; v τ (t) = ū, if t < t + τ; u(t τ), if t > t + τ. Then, it can be verified that Φv t+τ τ (x 0 ) = Φu t t 2 Ψ h Φ t u1 (x 0 ) = Ψ h Φ t u(x 0 ), (3) with h = exp(ητ), and η = g( t)ξ(z( t))g 1 ( t), i.e., the perturbed final state can be obtained by applying a certain group action to the unperturbed final state, corresponding to a flow along the vector field η for time τ 0. The same argument can be repeated for a finite number of times t 1,..., t k for which (2) holds; correspondingly, (3) can be written as Φv t+τ τ (x 0 ) = Ψ hk... Ψ h1 Φ t u(x 0 ). The significance of the above equation is that it shows that the perturbed final state can be obtained from the unperturbed final state, through the combination of (positive) flows along the vector fields η i. An immediate consequence of this is that, if the set {g G : g = i exp(η iτ i ), τ i 0, i} is equal to G, then gaps can be eliminated completely (ignoring obstacles). A catalog of vector fields that satisfy this conditions in cases of interest, together with formulas for inversion, is presented in [25]. V. IMPROVING PLANNERS BY GAP REDUCTION In this section, the gap reduction algorithm [6] is used to improve sampling based planners. As we mentioned in Section III, obtaining high precision solutions normally requires a small gap tolerance. According to our experimental results in Fig. 4, the computational cost increases dramatically with the gap tolerance decreasing. Therefore, the key idea for the improvement is to plan low precision solutions with big gaps, and then refine solution quality by reducing the gaps. Since it is relatively easy to find a low precision solution and the gap reduction algorithm is very efficient, a high precision solution could be returned much faster. Gaps induced by the gap tolerance will be reduced in two steps since these gaps exist in both the fiber and base space. The gaps in the base space are reduced first because the gap reduction algorithm in [6] reduces the gaps in the fiber space while maintaining the base variables. Then after the gaps in the fiber space are reduced in the second step, gaps in both fiber and base space are reduced. To simplify the first reduction step, we only consider the base dynamics while ignoring the fiber dynamics. Since only systems with affine in control base dynamics are considered in the paper, the base gap reduction is actually a control design problem for systems affine in control. Classical techniques for systems affine in control [13], [30] could be used to reduce the gaps in the base space and are not discussed here for the sake of conciseness. Modification to ISAs: The modification to the bidirectional tree based ISA is as follows and it could be easily extended to other cases. Let T 1 and T 2 be two trees generated from the initial and goal state, respectively. We can assume that in some iteration, T 1 generates a new node n new and ρ(x(n new ), x(n)) > ɛ s for any n in T 2. Normally, it will take much longer time for T 1 to generate n new such that ρ(x(n new), x(n)) < ɛ s for some n in T 2. Now for each node n in tree T 2, we reduce the gap between x(n new ) and x(n) in two steps shown above. If the final gap is less than ɛ s, then we find a solution. Note that since the fiber dynamics is ignored in the first step, the gaps in the fiber space could be either larger or smaller. We check the gap in fiber space after the first reduction. If it is less than an intermediate tolerance ɛ b, which is normally much larger than ɛ s, then the gap reduction algorithm [6] is called to reduce the gap in the fiber space. Modification to roadmap based planners: When analytical steering methods are not available, approximate planners (e.g., ISAs) could be used as local planners for roadmap based planners [1]. If the running time of local planners inceases dramatically with the gap tolerance decreasing, the performance of roadmap based planners to find high precision solutions will degrade greatly since both the construction and query depend on the local planner. Therefore, if the local planner could find higher precision solutions faster, the overall performance of roadmap based planners will be improved. Similar to the above idea, we could improve the local planner with the gap reduction algorithm. If big gaps in a low precision solution are successfully reduced to be within some given tolerance, then the roadmap is updated. In the roadmap construction phase, this may allow two milestones to be connected. In the query phase, same approach may be used to connect the initial and goal queries to the solution. Finally, when a solution is produced, the gap reduction may be further applied. VI. SYSTEM MODELS USED IN SIMULATIONS Our simulation includes three systems with affine-incontrol base dynamics, which are a car with dynamics, a car-trailer system, and an underactuated spacecraft.

5 Car with dynamics: The state vector, in coordinates, is (v y, ω, x, y, θ), in which x, y, and θ represent position and orientation; ω is the angular velocity, v y are translational velocity along the local y-axis fixed on the car. The input to the system is the steering angle u. The equations of motion are v y = v x ω + (f yf (u) + f yr (u))/m, ω = (f yf (u)a f yr (u)b)/i, ẋ = v x cos(θ) v y sin(θ), ẏ = v x sin(θ) + v y cos(θ), θ = ω, in which vx is constant translational velocity along local x-axis, M and I are the mass and inertia, and f yf (u) and f yr (u) are affine in u and represent forces acting on front and rear tires along the local y-axis. The fiber of the system is (x, y, θ), and the base is (v y, ω). The car-trailer system: The car-trailer system consists of a car pulling a trailer. The state of the system is (x, y, θ 1, v, θ 2 ), in which x, y, and θ 1 are position and orientation of the car, v is the translational velocity along local x-axis of the car, and θ 2 is the orientation of the trailer. The input to the system is (u 1, u 2 ), in which u 1 is the changing rate of v, and u 2 controls the steering angle. The motion equations of the trailer system are as follows: ẋ = v cos(θ 1 ), ẏ = v sin(θ 1 ), θ1 = vu 2 /L 1, v = u 1, θ 2 = v sin(θ 1 θ 2 )/L 2, in which L 1 is the length of the car, L 2 the length of the hitch. The system with affinein-control base dynamics could be seen by introducing the transformation θ d = θ 1 θ 2 and change the last equation above to θ d = vu 2 /L 1 v sin(θ d )/L 2. The fiber of the system is (x, y, θ 1 ) and the base is (v, θ d ). Note, we intentionally set v, the forward speed, to be nonnegative in our simulation so that the system is not STLC, and the gap cannot be reduced by trivially moving along the direction of Lie bracket. The spacecraft with orientation constraints: This problem is from [12] with some modifications. Dynamics of the spacecraft is considered. Also, we assume that some of the thrusters on the spacecraft (Fig. 3) do not work because of malfunction. The translational forces and rotational torques for the spacecraft will only be provided from three pairs of thrusters. Each pair could provide forces in opposite, independent directions, and these forces do not pass through the mass center; the system is hence underactuated but controllable. The state of the system is represented by (g, ξ), in which g = (p, R) SE(3) represents position, p R 3 and orientation, R SO(3), and ξ = (ˆΩ, V ) se(3) denote the translational and rotational velocity expressed in the body frame. The skew matrix ˆΩ is defined as unique matrix for which ˆΩv = Ω v, v R 3. The motion equations are: Ṙ = RˆΩ, ṗ = RV, Ω = J 1 (Ω (JΩ) f Ω ), V = V Ω + f V /M, in which J is the inertial matrix, M is the mass, f V = [u 0, u 1, u 2 ] T is the translational force vector, f Ω = [ u 2 L z, u 0 L x, u 1 L y ] T is the rotational torque vector and L x, L y, and L z are vertical distance from the mass center to the direction of forces generated by thrusters. Besides dynamics constraints and the space station in the environment, constraints also come from the star tracker, the telescope and the communication antenna on the spacecraft. Specifically, the antenna should not be too far away from the direction of the Earth to maintain Fig. 3. A spacecraft equipped with a star tracker, telescope and communication antenna. Note that this model is a fictional model. the communication. To protect sensitive equipments, the star tracker cannot be close to the direction of the Sun, and the telescope cannot be close to the direction of any bright objects (such as the Sun, Moon, Jupiter and Earth). In our implementation, we assume that the antenna is at the top of the spacecraft along the local z axis. The telescope and star tracker are along the local x and y axis, respectively. The light from bright objects comes in parallel from different directions. Specifically, sitting at the origin of the inertia frame, we can see that the light of the Earth, Sun, Jupiter and Moon is from the direction of (0, 0, 1), (1, 1, 1), (1, 1, 0), and (1, 0, 1), respectively. VII. SIMULATION STUDIES We did simulations with two sampling-based planners. One is an ISA based on Resolution Complete Rapidly- Exploring Random Trees (RC-RRTs) [7], and the other is a basic PRM-based planner. The simulation is done on a 2.0 Ghz PC running Linux. The NAG library is used to solve nonlinear programs in the gap reduction. A motion planning problem is constructed for each system from Section VI. The problems and sample solutions are shown in Figs. 5 and 1, respectively. The first set of simulations shows the relationship between the running time and the gap tolerance. The original RC-RRT based planner solves the same problems with gap tolerance of different sizes. For each gap tolerance, we run the planner 20 times and report the total running time. The smallest gap tolerance for problems with the car, the trailer, and the spacecraft is 0.37, 0.1, and 3.0, respectively. The results are shown in Fig. 4, from which we can see that the running time of the old planner increases dramatically with the gap tolerance decreasing. In the second set of simulations, we compare the running time to find solutions with similar gap tolerance. The search tolerance ɛ s for all problems is around 0.1. Both the old planner and the improved planner are used

6 Current gap toler. / Smallest gap toler. Model Fig. 4. Total running times in seconds for 20 trials to solve same problems with different gap tolerance, in which toler. means tolerance, and 1, 2 and 3 denote the problem with the car, the trailer, and the spacecraft, respectively. Improved Planner Old Planner Av. Max. Min. Av. Max. Min. (s.) (s.) (s.) (s.) (s.) (s.) N/A N/A N/A Fig. 6. Comparison of running time on different problems, in which 1, 2 and 3 represent the car, trailer, and spacecraft system, Av., Max. and Min. represent the average, maximum, and minimum running time, N/A means that no solution is found after 72 hours. Fig. 5. Trajectory design for the car with dynamics and the trailer. to solve same problems 40 times. The average, maximum and minimum running time are reported. The comparison of running time is given in Table 6. From the table, it is easy to see that the running time to find high precision solutions is considerably reduced. We also did simulation for spacecraft problem with the resolution of control space discretization increased by 4 times, but the original planner still did not return solution after 72 hours. Finally, the PRM planner is test on a problem with a unicycle car with the acceleration control. The car model is ẋ = v cos(θ), ẏ = v sin(θ), θ = vu 2 /L 1 and v = u 1, in which (x, y, θ) are the position and orientation, v is the forward speed (nonnegative in our simulation) in the local frame, L 1 is the length of the car, u 1, u 2 control the acceleration and steering angle, respectively. The above improved ISA is used as the local planner with the gap tolerance 0.1. In the construction stage, a directed edge is added between two states (including the velocity) when the local planner successfully connects them. In the multiquery stage, if both initial and goal states could be connected to the roadmap, a solution is returned. After building the roadmap for each 25 iterations, we will query solutions for 50 randomly chosen initial and goal state pairs. The results are shown in Fig. 8 and a partial roadmap is in Fig. 7. VIII. CONCLUSION This paper establishes the power of efficient gap reduction techniques in sampling-based motion planning under differential constraints. New planners are designed by combining the improved gap reduction algorithm [6] with an RC-RRT based planner in [7] and a simple PRM based planner. The comparison of RC-RRT based planners with or without gap reduction on the running time to solve same problems demonstrated dramatic performance improvements. We expect that techniques such as this Fig. 7. planner. A partial roadmap built by the PRM-based nonholonomic will enable many new challenging problems to be solved. Furthermore, the performance of the primitive PRM-based planner showed how the gap reduction techniques can be used to construct roadmaps without requiring an accurate steering method. However, substantial work remains to make this approach to PRMs viable for applications. ACKNOWLEDGMENTS This work was funded in part by the following grants: NSF CAREER (LaValle), NSF (Frazzoli and LaValle), and NSF (Bullo and LaValle). IX. REFERENCES [1] M. Akinc, K. Bekris, B. Chen, A. Ladd, E. Plakue, and L. Kavraki. Probabilistic roadmaps of trees for parallel computation of multiple query roadmaps. In Eleventh International Symposium on Robotics Research, [2] J. Barraquand and J.-C. Latombe. Nonholonomic multibody mobile robots: Controllability and motion planning in the presence of obstacles. Algorithmica, 10: , 1993.

7 Iter. C. T.(s) Q. T.(s) Success N.N. E.N / / / / / / / Fig. 8. Simulation results for the PRM based nonholonomic planner, in which Iter. means iteration, C.T. is the construction time, Q.T. is the overall query time for 50 trials, N.N. is the number of milestones, and E.N. is the number of edges in the roadmap. [3] F. Bullo and K. M. Lynch. Kinematic controllability for decoupled trajectory planning in underactuated mechanical systems. IEEE Trans. on Robotics and Automation, 17(4): , [4] J. Canny, A. Rege, and J. Reif. An exact algorithm for kinodynamic planning in the plane. Discrete and Computational Geometry, 6: , [5] J. F. Canny. The Complexity of Robot Motion Planning. MIT Press, Cambridge, MA, [6] P. Cheng, E. Frazzoli, and S. LaValle. Exploiting group symmetries to improve precision in kinodynamic and nonholonomic planning. In IEEE/RSJ Int. Conf. on Intelligent Robots & Systems, [7] P. Cheng and S. LaValle. Resolution complete rapidly-exploring random trees. In IEEE Int. Conf. Robot. & Autom., [8] P. Cheng and S. LaValle. Resolution completeness for sampling-based motion planning with differential constraints [9] B. R. Donald, P. G. Xavier, J. Canny, and J. Reif. Kinodynamic planning. Journal of the ACM, 40: , November [10] E. Frazzoli. Robust Hybrid Control for Autonomous Vehicle Motion Planning. Department of Aeronautics and Astronautics, Massachusetts Institute of Technology, Cambridge, MA, June [11] E. Frazzoli, M. A. Dahleh, and E. Feron. Realtime motion planning for agile autonomous vehicles. AIAA Journal of Guidance, Control, and Dynamics, 25(1): , [12] E. Frazzoli, M. A. Dahleh, E. Feron, and R. Kornfeld. A randomized attitude slew planning algorithm for autonomous spacecraft. In AIAA Guidance, Navigation, and Control Conference, [13] A. Isidori. Nonlinear Control Systems. Springer- Verlag, Berlin, [14] L. E. Kavraki, P. Svestka, J.-C. Latombe, and M. H. Overmars. Probabilistic roadmaps for path planning in high-dimensional configuration spaces. IEEE Trans. Robot. & Autom., 12(4): , June [15] J. Kim and J. P. Ostrowski. Motion planning of aerial robot using rapidly-exploring random trees with dynamic constraints. In IEEE Int. Conf. Robot. & Autom., [16] R. Kindel, D. Hsu, J.-C. Latombe, and S. Rock. Kinodynamic motion planning amidst moving obstacles. In IEEE Int. Conf. Robot. & Autom., [17] S. Kobayashi and K. Nomizu. Foundations of Differential Geometry. Vol. I, volume 15 of Interscience Tracts in Pure and Applied Mathematics. Interscience Publishers, New York, NY, [18] G. Laffierriere and H. J. Sussman. Nonholonomic Motion Planning, chapter A Differential Geometric Approach to Motion Planning. The Kluwer International Series in Engineering and Comuter Science, Boston, MA, [19] F. Lamiraux and D. Bonnafous. Reactive trajectory deformation for nonholonomic systems: Application to mobile robots. In IEEE Int. Conf. Robot. & Autom., pages , [20] J. P. Laumond, S. Sekhavat, and F. Lamiraux. Guidelines in nonholonomic motion planning for mobile robots. In J.-P. Laumond, editor, Robot Motion Plannning and Control, pages Springer-Verlag, Berlin, [21] S. M. LaValle and J. J. Kuffner. Rapidly-exploring random trees: Progress and prospects. In B. R. Donald, K. M. Lynch, and D. Rus, editors, Algorithmic and Computational Robotics: New Directions, pages A K Peters, Wellesley, MA, [22] K. M. Lynch and M. T. Mason. Stable pushing: Mechanics, controllability, and planning. Int. J. Robot. Res., 15(6): , [23] A. Marigo and A. Bicchi. Steering driftless nonholonomic systems by control quanta. In IEEE Conf. Decision & Control, [24] A. Marigo, B. Piccoli, and A. Bicchi. Reachability analysis for a class of quantized control systems. In Proc. IEEE Conf. on Decision and Control, [25] S. Martinez, J. Cortes, and F. Bullo. A catalog of inverse-kinematics planners for underactuated systems on matrix lie groups. In IEEE/RSJ Int. Conf. on Intelligent Robots & Systems, pages , [26] R. M. Murray and S. Sastry. Nonholonomic motion planning: Steering using sinusoids. Trans. Automatic Control, 38(5): , [27] J. A. Reeds and L. A. Shepp. Optimal paths for a car that goes both forwards and backwards. Pacific J. Math., 145(2): , [28] J. Reif and H. Wang. Non-uniform discretization approximations for kinodynamic motion planning. In J.-P. Laumond and M. Overmars, editors, Algorithms for Robotic Motion and Manipulation, pages A K Peters, Wellesley, MA, [29] P. Rouchon, M. Fliess, M. Levine, and P. Martin. Flatness, motion planning, and trailer systems. In Proc. IEEE Conf. on Decsion and Control, pages , [30] S. Sastry. Nonlinear systems, analysis, stability and control. Springer, New York, NY, [31] S. Sekhavat, P. Svestka, J.-P. Laumond, and M. H. Overmars. Multilevel path planning for nonholonomic robots using semiholonomic subsystems. Int. J. Robot. Res., 17: , 1998.