Emilio Frazzoli

Sampling-based algorithms for optimal motion planning

During the last decade, sampling-based path planning algorithms, such as probabilistic roadmaps (PRM) and rapidly exploring random trees (RRT), have been shown to work well in practice and possess theoretical guarantees such as probabilistic completeness. However, little effort has been devoted to the formal analysis of the quality of the solution returned by such algorithms, e.g. as a function of the number of samples. The purpose of this paper is to fill this gap, by rigorously analyzing the asymptotic behavior of the cost of the solution returned by stochastic sampling-based algorithms as the number of samples increases. A number of negative results are provided, characterizing existing algorithms, e.g. showing that, under mild technical conditions, the cost of the solution returned by broadly used sampling-based algorithms converges almost surely to a non-optimal value. The main contribution of the paper is the introduction of new algorithms, namely, PRM* and RRT*, which are provably asymptotically optimal, i.e. such that the cost of the returned solution converges almost surely to the optimum. Moreover, it is shown that the computational complexity of the new algorithms is within a constant factor of that of their probabilistically complete (but not asymptotically optimal) counterparts. The analysis in this paper hinges on novel connections between stochastic sampling-based path planning algorithms and the theory of random geometric graphs.

Real-Time Motion Planning for Agile Autonomous Vehicles

Planning the path of an autonomous, agile vehicle in a dynamic environment is a very complex problem, especially when the vehicle is required to use its full maneuvering capabilities. Recent efforts aimed at using randomized algorithms for planning the path of kinematic and dynamic vehicles have demonstrated considerable potential for implementation on future autonomous platforms. This paper builds upon these efforts by proposing a randomized path planning architecture for dynamical systems in the presence of fixed and moving obstacles. This architecture addresses the dynamic constraints on the vehicles motion, and it provides at the same time a consistent decoupling between low-level control and motion planning. The path planning algorithm retains the convergence properties of its kinematic counterparts. System safety is also addressed in the face of finite computation times by analyzing the behavior of the algorithm when the available onboard computation resources are limited, and the planning must be performed in real time. The proposed algorithm can be applied to vehicles whose dynamics are described either by ordinary differential equations or by higher-level, hybrid representations. Simulation examples involving a ground robot and a small autonomous helicopter are presented and discussed.

Maneuver-based motion planning for nonlinear systems with symmetries

In this paper, we introduce an approach for the efficient solution of motion-planning problems for time-invariant dynamical control systems with symmetries, such as mobile robots and autonomous vehicles, under a variety of differential and algebraic constraints on the state and on the control inputs. Motion plans are described as the concatenation of a number of well-defined motion primitives, selected from a finite library. Rules for the concatenation of primitives are given in the form of a regular language, defined through a finite-state machine called a Maneuver Automaton. We analyze the reachability properties of the language, and present algorithms for the solution of a class of motion-planning problems. In particular, it is shown that the solution of steering problems for nonlinear dynamical systems with symmetries and invariant constraints can be reduced to the solution of a sequence of kinematic inversion problems. A detailed example of the application of the proposed approach to motion planning for a small aerobatic helicopter is presented.

Symbolic planning and control of robot motion [Grand Challenges of Robotics]

In this paper, different research trends that use symbolic techniques for robot motion planning and control are illustrated. As it often happens in new research areas, contributions to this topic started at about the same time by different groups with different emphasis, approaches, and notation. This article tries to describe a framework in which many of the current methods and ideas can be placed and to provide a coherent picture of what the authors want to do, what have they got so far, and what the main missing pieces are. Generally speaking, the aim of symbolic control as is envisioned in this article is to enable the usage of methods of formal logic, languages, and automata theory for solving effectively complex planning problems for robots and teams of robots. The results presented in this article can be divided in two groups: top-down approaches, whereby formal logic tools are employed on rather abstract models of robots; and bottom up approaches, whose aim is to provide means by which such abstractions are possible and effective. The two ends do not quite tie as yet, and much work remains to be done in both directions to obtain generally applicable methods. However, the prospects of symbolic control of robots are definitely promising, and the challenging nature of problems to be solved warrants for the interest of a wide community of researchers

Robust hybrid control for autonomous vehicle motion planning

The operation of an autonomous vehicle in an unknown, dynamic environment is a very complex problem, especially when the vehicle is required to use its full maneuvering capabilities, and to react in real time to changes in the operational environment. A possible approach to reduce the computational complexity of the motion planning problem for a nonlinear, high dimensional system, is based on a quantization of the system dynamics, leading to a control architecture based on a hybrid automaton, the states of which represent feasible trajectory primitives for the vehicle. The paper focuses on the feasibility of this approach, in the presence of disturbances and uncertainties in the plant and/or in the environment: the structure of a robust hybrid automaton is defined and its properties are analyzed. In particular, we address the issues of well-posedness, consistency and reachability. For the case of autonomous vehicles, we provide sufficient conditions to guarantee reachability of the automaton.

Incremental Sampling-based Algorithms for Optimal Motion Planning.

During the last decade, incremental sampling-based motion planning algorithms, such as the Rapidly-exploring Random Trees (RRTs), have been shown to work well in practice and to possess theoretical guarantees such as probabilistic completeness. However, no theoretical bounds on the quality of the solution obtained by these algorithms, e.g., in terms of a given cost function, have been established so far. The purpose of this paper is to fill this gap, by designing efficient incremental samplingbased algorithms with provable optimality properties. The first contribution of this paper is a negative result: it is proven that, under mild technical conditions, the cost of the best path returned by RRT converges almost surely to a non-optimal value, as the number of samples increases. Second, a new algorithm is considered, called the Rapidly-exploring Random Graph (RRG), and it is shown that the cost of the best path returned by RRG converges to the optimum almost surely. Third, a tree version of RRG is introduced, called RRT∗, which preserves the asymptotic optimality of RRG while maintaining a tree structure like RRT. The analysis of the new algorithms hinges on novel connections between sampling-based motion planning algorithms and the theory of random geometric graphs. In terms of computational complexity, it is shown that the number of simple operations required by both the RRG and RRT∗ algorithms is asymptotically within a constant factor of that required by RRT.

Trajectory tracking control design for autonomous helicopters using a backstepping algorithm

In this paper we present a tracking controller for a class of underactuated mechanical systems, based on a backstepping procedure. This class includes an approximation of small helicopter dynamics. The need to avoid artificial singularities due to the attitude representation is the main driver behind the control design presented in this paper: to achieve this goal, we will operate directly in the configuration manifold of the vehicle. The control design provides asymptotic tracking for an approximate model of small helicopters, and bounded tracking when more complete models are considered. Simulation examples, including both point stabilization and aggressive maneuver tracking, are presented and discussed.

Anytime Motion Planning using the RRT

The Rapidly-exploring Random Tree (RRT) algorithm, based on incremental sampling, efficiently computes motion plans. Although the RRT algorithm quickly produces candidate feasible solutions, it tends to converge to a solution that is far from optimal. Practical applications favor “anytime” algorithms that quickly identify an initial feasible plan, then, given more computation time available during plan execution, improve the plan toward an optimal solution. This paper describes an anytime algorithm based on the RRT* which (like the RRT) finds an initial feasible solution quickly, but (unlike the RRT) almost surely converges to an optimal solution. We present two key extensions to the RRT*, committed trajectories and branch-and-bound tree adaptation, that together enable the algorithm to make more efficient use of computation time online, resulting in an anytime algorithm for real-time implementation. We evaluate the method using a series of Monte Carlo runs in a high-fidelity simulation environment, and compare the operation of the RRT and RRT* methods. We also demonstrate experimental results for an outdoor wheeled

Decentralized Cooperative Policy for Conflict Resolution in Multivehicle Systems

In this paper, we propose a novel policy for steering multiple vehicles between assigned start and goal configurations, ensuring collision avoidance. The policy rests on the assumption that all agents are cooperating by implementing the same traffic rules. However, the policy is completely decentralized, as each agent decides its own motion by applying those rules only on the locally available information, and scalable, in the sense that the amount of information processed by each agent and the computational complexity of the algorithms do not increase with the number of agents in the scenario. The proposed policy applies to systems in which new vehicles may enter the scene and start interacting with existing ones at any time, while others may leave. Under mild conditions on the initial configurations, the policy is shown to be safe, i.e., it guarantees collision avoidance throughout the system evolution. In the paper, conditions are discussed on the desired configurations of agents, under which the ultimate convergence of all vehicles to their goals can also be guaranteed. To show that such conditions are actually necessary and sufficient, which turns out to be a challenging liveness-verification problem for a complex hybrid automaton, we employ a probabilistic verification method. The paper finally presents and discusses simulations for systems of several tens of vehicles, and reports on some experimental implementation showing the practicality of the approach.

Traveling Salesperson Problems for the Dubins Vehicle

In this paper, we study minimum-time motion planning and routing problems for the Dubins vehicle, i.e., a nonholonomic vehicle that is constrained to move along planar paths of bounded curvature, without reversing direction. Motivated by autonomous aerial vehicle applications, we consider the traveling salesperson problem for the Dubins vehicle (DTSP): given n points on a plane, what is the shortest Dubins tour through these points, and what is its length? First, we show that the worst-case length of such a tour grows linearly with n and we propose a novel algorithm with performance within a constant factor of the optimum for the worst-case point sets. In doing this, we also obtain an upper bound on the optimal length in the classical point-to-point problem. Second, we study a stochastic version of the DTSP where the n targets are randomly and independently sampled from a uniform distribution. We show that the expected length of such a tour is of order at least n 2/3 and we propose a novel algorithm yielding a solution with length of order n 2/3 with probability one. Third and finally, we study a dynamic version of the DTSP: given a stochastic process that generates target points, is there a policy that guarantees that the number of unvisited points does not diverge over time? If such stable policies exist, what is the minimum expected time that a newly generated target waits before being visited by the vehicle? We propose a novel stabilizing algorithm such that the expected wait time is provably within a constant factor from the optimum.