This paper presents an efficient optimization framework that solves trajectory optimization problems efficiently by decoupling state variables from timing variables, thereby decomposing a challenging nonlinear programming (NLP) problem into two easier subproblems. With timing fixed, the state variables can be optimized efficiently using convex optimization, and so the time variables can be optimized using a separate outer optimization. This is a bilevel optimization in which the outer objective function itself requires an optimization to compute. The challenge is that gradient optimization methods require the gradient of the objective function with respect to the time variables, which is not available. Whereas the finite difference method must solve many optimization problems to compute a gradient, this paper proposes a more efficient method: the dual solution (Lagrange multipliers) of the convex optimization problem is exploited to calculate the analytical gradient. Since the dual solution is a by-product of the convex optimization problem, the gradient can be obtained for free' with high accuracy. The framework is demonstrated on solving minimum-jerk trajectory optimization problems in safety corridors for unmanned aerial vehicles (UAVs). Experiments demonstrate that bilevel optimization improves performance over a standard NLP solver, and analytical gradients outperforms finite differences. With a 40 ,ms cutoff time, our approach achieves over 8 times better suboptimality than the current state-of-the-art.
Richter @cite_13 proposes a framework that is able to generate collision-free spline trajectories and optimize time allocation. They managed to formulate a QP with only equality constraints. However, such a formulation has the disadvantage: (a) Being collision-free is achieved by iteratively adding new points and solve another QP if there is a collision. This strategy has no bound on the number of iterations needed though their experiments show a few iterations are usually enough. (b) Dynamic feasibility is achieved by checking the max acceleration in every iteration and stop the iteration if the acceleration reaches the limit. Since computing the extrema of a high order polynomial is hard according to the Abel-Ruffini theorem, this strategy can be slow when we are using high order polynomials. Moreover, their framework solves the optimization problem by re-formulating the equality constrained QP into an unconstrained one, they will not be able to handle general inequality constraints which often exist in trajectory optimization. In our work, we use bilevel optimization as a way to solve a GBD, and exploit theoretical developments from these fields to derive analytical gradients for trajectory optimization.
@cite_13
2587415290 1605218591 2890926967 1937654348
There is extensive literature on using convex optimization to derive piece-wise polynomial trajectories for controlling differential flat systems with applications to three-dimensional flight for Micro Aerial Vehicles. In this work, we propose a method to formulate trajectory generation as a quadratic program (QP) using the concept of a Safe Flight Corridor (SFC). The SFC is a collection of convex overlapping polyhedra that models free space and provides a connected path from the robot to the goal position. We derive an efficient convex decomposition method that builds the SFC from a piece-wise linear skeleton obtained using a fast graph search technique. The SFC provides a set of linear inequality constraints in the QP allowing real-time motion planning. Because the range and field of view of the robot's sensors are limited, we develop a framework of Receding Horizon Planning , which plans trajectories within a finite footprint in the local map, continuously updating the trajectory through a re-planning process. The re-planning process takes between 50 to 300 ms for a large and cluttered map. We show the feasibility of our approach, its completeness and performance, with applications to high-speed flight in both simulated and physical experiments using quadrotors. We present a new approach to the design of smooth trajectories for quadrotor unmanned aerial vehicles (UAVs), which are free of collisions with obstacles along their entire length. To avoid the non-convex constraints normally required for obstacle-avoidance, we perform a mixed-integer optimization in which polynomial trajectories are assigned to convex regions which are known to be obstacle-free. Prior approaches have used the faces of the obstacles themselves to define these convex regions. We instead use IRIS, a recently developed technique for greedy convex segmentation [1], to pre-compute convex regions of safe space. This results in a substantially reduced number of integer variables, which improves the speed with which the optimization can be solved to its global optimum, even for tens or hundreds of obstacle faces. In addition, prior approaches have typically enforced obstacle avoidance at a finite set of sample or knot points. We introduce a technique based on sums-of-squares (SOS) programming that allows us to ensure that the entire piecewise polynomial trajectory is free of collisions using convex constraints. We demonstrate this technique in 2D and in 3D using a dynamical model in the Drake toolbox for Matlab [2]. We focus on a replanning scenario for quadrotors where considering time efficiency, non-static initial state and dynamical feasibility is of great significance. We propose a real-time B-spline based kinodynamic (RBK) search algorithm, which transforms a position-only shortest path search (such as A * and Dijkstra) into an efficient kinodynamic search, by exploring the properties of B-spline parameterization. The RBK search is greedy and produces a dynamically feasible time-parameterized trajectory efficiently, which facilitates non-static initial state of the quadrotor. To cope with the limitation of the greedy search and the discretization induced by a grid structure, we adopt an elastic optimization (EO) approach as a post-optimization process, to refine the control point placement provided by the RBK search. The EO approach finds the optimal control point placement inside an expanded elastic tube which represents the free space, by solving a Quadratically Constrained Quadratic Programming (QCQP) problem. We design a receding horizon replanner based on the local control property of B-spline. A systematic comparison of our method against two state-of-the-art methods is provided. We integrate our replanning system with a monocular vision-based quadrotor and validate our performance onboard. The thesis describes a new method for obtaining minimizers for optimal control problems whose minima serve as control policies for guiding nonlinear dynamical systems to achieve prescribed goals under imposed trajectory and actuator constraints. One of the major contributions of the present work resides in the approximation of such minimizers by piecewise polynomial functions expressed in terms of a linear combination of non-uniform rational B-spline (NURBS) basis functions and the judicious exploitation of the properties of the resulting NURBS curves to improve the computational effort often associated with solving optimal control problems for constrained dynamical systems. In particular, by exploiting the two structures combined in a NURBS curve, NURBS basis functions and an associated union of overlapping polytopes constructed from the coefficients of the linear combination, we are able to separate an optimal control problem into two subproblems | guidance and obstacle avoidance, making the original problem tractable. This is accomplished by laying out the union of overlapping polytopes in such a way that they delineate a section of space that avoids all obstacles and then manipulating the NURBS basis functions to obtain trajectories that are guaranteed to remain bounded by this section of space without explicitly including the conjunction of disjunctions naturally induced from obstacles into the guidance problem. In addition, we show how one can construct systematically a feasible region that corresponds to a NURBS parameterization starting from an ordered union of pairwise adjacently overlapping nonempty compact convex sets. Specifically, we show how to setup a nonlinear programming problem to solve for the feasible region in terms of an ordered union of pairwise adjacently overlapping polytopes with nonempty interiors by maximizing the sum of their volumes and starting from a feasible region described by an ordered union of pairwise adjacently overlapping nonempty convex compact simi-algebraic sets. Finally, we show how this strategy can be implemented practically for an autonomous system traversing an urban environment. Finally, this work culminated in the filing of patent US20070179685 on behalf of Northrop Grumman for the Space Technology sector and in the development of the NURBS-based OTG software package. This C++ package contains the theoretical results of this thesis in the form of an object-oriented implementation optimized for real-time trajectory generation.