Robust online motion planning via contraction theory and convex optimization
01 May 2017-pp 5883-5890
TL;DR: This work presents a framework for online generation of robust motion plans for robotic systems with nonlinear dynamics subject to bounded disturbances, control constraints, and online state constraints such as obstacles and demonstrates the approach through simulations of a 6-state planar quadrotor navigating cluttered environments in the presence of a cross-wind.
Abstract: We present a framework for online generation of robust motion plans for robotic systems with nonlinear dynamics subject to bounded disturbances, control constraints, and online state constraints such as obstacles. In an offline phase, one computes the structure of a feedback controller that can be efficiently implemented online to track any feasible nominal trajectory. The offline phase leverages contraction theory and convex optimization to characterize a fixed-size “tube” that the state is guaranteed to remain within while tracking a nominal trajectory (representing the center of the tube). In the online phase, when the robot is faced with obstacles, a motion planner uses such a tube as a robustness margin for collision checking, yielding nominal trajectories that can be safely executed, i.e., tracked without collisions under disturbances. In contrast to recent work on robust online planning using funnel libraries, our approach is not restricted to a fixed library of maneuvers computed offline and is thus particularly well-suited to applications such as UAV flight in densely cluttered environments where complex maneuvers may be required to reach a goal. We demonstrate our approach through simulations of a 6-state planar quadrotor navigating cluttered environments in the presence of a cross-wind. We also discuss applications of our approach to Tube Model Predictive Control (TMPC) and compare the merits of our method with state-of-the-art nonlinear TMPC techniques.
Citations
More filters
21 Mar 2017
TL;DR: In this paper, the authors present the Fast and Safe Tracking (FaSTrack) algorithm for real-time planning of dynamical systems through a priori unknown cluttered environments, which provides a safety controller for the vehicle along with a guaranteed tracking error bound.
Abstract: Fast and safe navigation of dynamical systems through a priori unknown cluttered environments is vital to many applications of autonomous systems. However, trajectory planning for autonomous systems is computationally intensive, often requiring simplified dynamics that sacrifice safety and dynamic feasibility in order to plan efficiently. Conversely, safe trajectories can be computed using more sophisticated dynamic models, but this is typically too slow to be used for real-time planning. We present the new algorithm FaSTrack: Fast and Safe Tracking. A path or trajectory planner using simplified dynamics to plan quickly can be incorporated into the FaSTrack framework, which provides a safety controller for the vehicle along with a guaranteed tracking error bound. This bound captures all possible deviations due to high dimensional dynamics and external disturbances. FaSTrack is modular and can be used with most current path or trajectory planners. We demonstrate this framework using a 10D nonlinear quadrotor model tracking a 3D path obtained from an RRT planner.
176 citations
TL;DR: A path or trajectory planner using simplified dynamics to plan quickly can be incorporated into the FaSTrack framework, which provides a safety controller for the vehicle along with a guaranteed tracking error bound.
Abstract: Fast and safe navigation of dynamical systems through a priori unknown cluttered environments is vital to many applications of autonomous systems. However, trajectory planning for autonomous systems is computationally intensive, often requiring simplified dynamics that sacrifice safety and dynamic feasibility in order to plan efficiently. Conversely, safe trajectories can be computed using more sophisticated dynamic models, but this is typically too slow to be used for real-time planning. We propose a new algorithm FaSTrack: Fast and Safe Tracking for High Dimensional systems. A path or trajectory planner using simplified dynamics to plan quickly can be incorporated into the FaSTrack framework, which provides a safety controller for the vehicle along with a guaranteed tracking error bound. This bound captures all possible deviations due to high dimensional dynamics and external disturbances. Note that FaSTrack is modular and can be used with most current path or trajectory planners. We demonstrate this framework using a 10D nonlinear quadrotor model tracking a 3D path obtained from an RRT planner.
144 citations
Cites methods from "Robust online motion planning via c..."
...A similar new approach, based on contraction theory and convex optimization, allows computation of error bounds that can then define safe tubes around a nominal dynamic trajectory computable online [18]....
[...]
TL;DR: In this article, the shape of the tube is based on an offline computed incremental Lyapunov function with a corresponding (nonlinear) incrementally stabilizing feedback, and the online optimization only implicitly includes these nonlinear functions in terms of scalar bounds.
Abstract: In this article, we present a nonlinear robust model predictive control (MPC) framework for general (state and input dependent) disturbances. This approach uses an online constructed tube in order to tighten the nominal (state and input) constraints. To facilitate an efficient online implementation, the shape of the tube is based on an offline computed incremental Lyapunov function with a corresponding (nonlinear) incrementally stabilizing feedback. Crucially, the online optimization only implicitly includes these nonlinear functions in terms of scalar bounds, which enables an efficient implementation. Furthermore, to account for an efficient evaluation of the worst case disturbance, a simple function is constructed offline that upper bounds the possible disturbance realizations in a neighborhood of a given point of the open-loop trajectory. The resulting MPC scheme ensures robust constraint satisfaction and practical asymptotic stability with a moderate increase in the online computational demand compared to a nominal MPC. We demonstrate the applicability of the proposed framework in comparison to state-of-the-art robust MPC approaches with a nonlinear benchmark example.
98 citations
TL;DR: This work proposes a new robust setpoint tracking MPC algorithm, which achieves reliable and safe tracking of a dynamic setpoint while guaranteeing stability and constraint satisfaction and is the first to show that both the proposed robust and approximate MPC schemes scale to real-world robotic systems.
Abstract: Fast feedback control and safety guarantees are essential in modern robotics. We present an approach that achieves both by combining novel robust model predictive control (MPC) with function approximation via (deep) neural networks (NNs). The result is a new approach for complex tasks with nonlinear, uncertain, and constrained dynamics as are common in robotics. Specifically, we leverage recent results in MPC research to propose a new robust setpoint tracking MPC algorithm, which achieves reliable and safe tracking of a dynamic setpoint while guaranteeing stability and constraint satisfaction. The presented robust MPC scheme constitutes a one-layer approach that unifies the often separated planning and control layers, by directly computing the control command based on a reference and possibly obstacle positions. As a separate contribution, we show how the computation time of the MPC can be drastically reduced by approximating the MPC law with a NN controller. The NN is trained and validated from offline samples of the MPC, yielding statistical guarantees, and used in lieu thereof at run time. Our experiments on a state-of-the-art robot manipulator are the first to show that both the proposed robust and approximate MPC schemes scale to real-world robotic systems.
81 citations
Cites methods from "Robust online motion planning via c..."
...Approaches making use of robust MPC schemes are not widely used in robotics (yet), but tube and funnel approaches have recently been explored for robust robot motion planning [17], [18], [19]....
[...]
21 May 2018
TL;DR: In this article, the authors introduce the notion of "meta-planning" in which a refined offline computation enables safe switching between different online planners, which provides autonomous systems with the ability to adapt motion plans to a priori unknown environments in real-time as sensor measurements detect new obstacles.
Abstract: Motion planning is an extremely well-studied problem in the robotics community, yet existing work largely falls into one of two categories: computationally efficient but with few if any safety guarantees, or able to give stronger guarantees but at high computational cost. This work builds on a recent development called FaSTrack in which a slow offline computation provides a modular safety guarantee for a faster online planner. We introduce the notion of “meta-planning” in which a refined offline computation enables safe switching between different online planners. This provides autonomous systems with the ability to adapt motion plans to a priori unknown environments in real-time as sensor measurements detect new obstacles, and the flexibility to maneuver differently in the presence of obstacles than they would in free space, all while maintaining a strict safety guarantee. We demonstrate the meta-planning algorithm both in simulation and in hardware using a small Crazyflie 2.0 quadrotor.
77 citations
References
More filters
Book•
01 Dec 1979
TL;DR: Spivak's comprehensive introduction to differential geometry as discussed by the authors takes as its theme the classical roots of contemporary differential geometry, and explains why it is absurdly inefficient to eschew the modern language of manifolds, bundles, forms, etc., which was developed precisely to rigorize the concepts of classical differential geometry.
Abstract: Spivak's Comprehensive introduction takes as its theme the classical roots of contemporary differential geometry. Spivak explains his Main Premise (my term) as follows: "in order for an introduction to differential geometry to expose the geometric aspect of the subject, an historical approach is necessary; there is no point in introducing the curvature tensor without explaining how it was invented and what it has to do with curvature". His second premise concerns the manner in which the historical material should be presented: "it is absurdly inefficient to eschew the modern language of manifolds, bundles, forms, etc., which was developed precisely in order to rigorize the concepts of classical differential geometry". Here, Spivak is addressing "a dilemma which confronts anyone intent on penetrating the mysteries of differential geometry". On the one hand, the subject is an old one, dating, as we know it, from the works of Gauss and Riemann, and possessing a rich classical literature. On the other hand, the rigorous and systematic formulations in current use were established relatively recently, after topological techniques had been sufficiently well developed to provide a base for an abstract global theory; the coordinate-free geometric methods of E. Cartan were also a major source. Furthermore, the viewpoint of global structure theory now dominates the subject, whereas differential geometers were traditionally more concerned with the local study of geometric objects. Thus it is possible and not uncommon for a modern geometric education to leave the subject's classical origins obscure. Such an approach can offer the great advantages of elegance, efficiency, and direct access to the most active areas of modern research. At the same time, it may strike the student as being frustratingly incomplete. As Spivak remarks, "ignorance of the roots of the subject has its price-no one denies that modern formulations are clear, elegant and precise; it's just that it's impossible to comprehend how any one ever thought of them." While Spivak's impulse to mediate between the past and the present is a natural one and is by no means unique, his undertaking is remarkable for its ambitious scope. Acting on its second premise, the Comprehensive introduction opens with an introduction to differentiable manifolds; the remaining four volumes are devoted to a geometric odyssey which starts with Gauss and Riemann, and ends with the Gauss-Bonnet-Chern Theorem and characteristic classes. A formidable assortment of topics is included along the way, in which we may distinguish several major historical themes: In the first place, the origins of fundamental geometric concepts are investigated carefully. As just one example, Riemannian sectional curvature is introduced by a translation and close exposition of the text of Riemann's remarkable paper, Über die Hypothesen, welche der Geometrie zu Grunde
3,840 citations
TL;DR: In this paper, the authors presented the first randomized approach to kinodynamic planning (also known as trajectory planning or trajectory design), where the task is to determine control inputs to drive a robot from an unknown position to an unknown target.
Abstract: This paper presents the first randomized approach to kinodynamic planning (also known as trajectory planning or trajectory design). The task is to determine control inputs to drive a robot from an ...
2,993 citations
"Robust online motion planning via c..." refers background in this paper
..., a standard sampling-based planner [18], [19]) that computes a nominal input u∗ online (i....
[...]
TL;DR: An SQP algorithm that uses a smooth augmented Lagrangian merit function and makes explicit provision for infeasibility in the original problem and the QP subproblems is discussed.
Abstract: Sequential quadratic programming (SQP) methods have proved highly effective for solving constrained optimization problems with smooth nonlinear functions in the objective and constraints. Here we consider problems with general inequality constraints (linear and nonlinear). We assume that first derivatives are available and that the constraint gradients are sparse.
We discuss an SQP algorithm that uses a smooth augmented Lagrangian merit function and makes explicit provision for infeasibility in the original problem and the QP subproblems. SNOPT is a particular implementation that makes use of a semidefinite QP solver. It is based on a limited-memory quasi-Newton approximation to the Hessian of the Lagrangian and uses a reduced-Hessian algorithm (SQOPT) for solving the QP subproblems. It is designed for problems with many thousands of constraints and variables but a moderate number of degrees of freedom (say, up to 2000). An important application is to trajectory optimization in the aerospace industry. Numerical results are given for most problems in the CUTE and COPS test collections (about 900 examples).
2,831 citations
"Robust online motion planning via c..." refers methods in this paper
...We solve the resulting problem using the SNOPT solver [32]....
[...]
TL;DR: An SQP algorithm that uses a smooth augmented Lagrangian merit function and makes explicit provision for infeasibility in the original problem and the QP subproblems is discussed and a reduced-Hessian semidefinite QP solver (SQOPT) is discussed.
Abstract: Sequential quadratic programming (SQP) methods have proved highly effective for solving constrained optimization problems with smooth nonlinear functions in the objective and constraints. Here we consider problems with general inequality constraints (linear and nonlinear). We assume that first derivatives are available and that the constraint gradients are sparse. Second derivatives are assumed to be unavailable or too expensive to calculate.
We discuss an SQP algorithm that uses a smooth augmented Lagrangian merit function and makes explicit provision for infeasibility in the original problem and the QP subproblems. The Hessian of the Lagrangian is approximated using a limited-memory quasi-Newton method.
SNOPT is a particular implementation that uses a reduced-Hessian semidefinite QP solver (SQOPT) for the QP subproblems. It is designed for problems with many thousands of constraints and variables but is best suited for problems with a moderate number of degrees of freedom (say, up to 2000). Numerical results are given for most of the CUTEr and COPS test collections (about 1020 examples of all sizes up to 40000 constraints and variables, and up to 20000 degrees of freedom).
2,205 citations
Book•
01 Dec 2004TL;DR: This paper recalls a few past achievements in Model Predictive Control, gives an overview of some current developments and suggests a few avenues for future research.
Abstract: This paper recalls a few past achievements in Model Predictive Control, gives an overview of some current developments and suggests a few avenues for future research.
1,897 citations
"Robust online motion planning via c..." refers methods in this paper
...Specifically, due to the difficulty of constructing invariant tubes and associated tracking controllers for nonlinear systems [10], most existing schemes for TMPC for nonlinear systems involve applying methods from linear TMPC by decomposing the dynamics into a linear and a nonlinear component (which is treated as a bounded disturbance) [11], [12]....
[...]