Solving Trajectory Optimization Problems Research Articles

Direct Collocation Methods for Trajectory Optimization in Constrained Robotic Systems

Direct collocation methods are powerful tools to solve trajectory optimization problems in robotics. While their resulting trajectories tend to be dynamically accurate, they may also present large kinematic errors in the case of constrained mechanical systems, i.e., those whose state coordinates are subject to holonomic or nonholonomic constraints, like loop-closure or rolling-contact constraints. These constraints confine the robot trajectories to an implicitly-defined manifold, which complicates the computation of accurate solutions. Discretization errors inherent to the transcription of the problem easily make the trajectories drift away from this manifold, which results in physically inconsistent motions that are difficult to track with a controller. This paper reviews existing methods to deal with this problem and proposes new ones to overcome their limitations. Current approaches either disregard the kinematic constraints (which leads to drift accumulation) or modify the system dynamics to keep the trajectory close to the manifold (which adds artificial forces or energy dissipation to the system). The methods we propose, in contrast, achieve full drift elimination on the discrete trajectory, or even along the continuous one, without artificial modifications of the system dynamics. We illustrate and compare the methods using various examples of different complexity.

Algorithmic differentiation improves the computational efficiency of OpenSim-based trajectory optimization of human movement.

Algorithmic differentiation (AD) is an alternative to finite differences (FD) for evaluating function derivatives. The primary aim of this study was to demonstrate the computational benefits of using AD instead of FD in OpenSim-based trajectory optimization of human movement. The secondary aim was to evaluate computational choices including different AD tools, different linear solvers, and the use of first- or second-order derivatives. First, we enabled the use of AD in OpenSim through a custom source code transformation tool and through the operator overloading tool ADOL-C. Second, we developed an interface between OpenSim and CasADi to solve trajectory optimization problems. Third, we evaluated computational choices through simulations of perturbed balance, two-dimensional predictive simulations of walking, and three-dimensional tracking simulations of walking. We performed all simulations using direct collocation and implicit differential equations. Using AD through our custom tool was between 1.8 ± 0.1 and 17.8 ± 4.9 times faster than using FD, and between 3.6 ± 0.3 and 12.3 ± 1.3 times faster than using AD through ADOL-C. The linear solver efficiency was problem-dependent and no solver was consistently more efficient. Using second-order derivatives was more efficient for balance simulations but less efficient for walking simulations. The walking simulations were physiologically realistic. These results highlight how the use of AD drastically decreases computational time of trajectory optimization problems as compared to more common FD. Overall, combining AD with direct collocation and implicit differential equations decreases the computational burden of trajectory optimization of human movement, which will facilitate their use for biomechanical applications requiring the use of detailed models of the musculoskeletal system.

Solving Trajectory Optimization Problems in the Presence of Probabilistic Constraints.

The objective of this paper is to present an approximation-based strategy for solving the problem of nonlinear trajectory optimization with the consideration of probabilistic constraints. The proposed method defines a smooth and differentiable function to replace probabilistic constraints by the deterministic ones, thereby converting the chance-constrained trajectory optimization model into a parametric nonlinear programming model. In addition, it is proved that the approximation function and the corresponding approximation set will converge to that of the original problem. Furthermore, the optimal solution of the approximated model is ensured to converge to the optimal solution of the original problem. Numerical results, obtained from a new chance-constrained space vehicle trajectory optimization model and a 3-D unmanned vehicle trajectory smoothing problem, verify the feasibility and effectiveness of the proposed approach. Comparative studies were also carried out to show the proposed design can yield good performance and outperform other typical chance-constrained optimization techniques investigated in this paper.

DirCol5i: Trajectory Optimization for Problems With High-Order Derivatives

In this technical brief, we focus on solving trajectory optimization problems that have nonlinear system dynamics and that include high-order derivatives in the objective function. This type of problem comes up in robotics—for example, when computing minimum-snap reference trajectories for a quadrotor or computing minimum-jerk trajectories for a robot arm. DirCol5i is a transcription method that is specialized for solving this type of problem. It uses the fifth-order splines and analytic differentiation to compute higher-derivatives, rather than using a chain-integrator as would be required by traditional methods. We compare DirCol5i to traditional transcription methods. Although it is slower for some simple optimization problems, when solving problems with high-order derivatives DirCol5i is faster, more numerically robust, and does not require setting up a chain integrator.

A full-space quasi-Lagrange-Newton-Krylov algorithm for trajectory optimization problems

The objectives of this work are to study and to apply the full-space quasi-Lagrange-Newton-Krylov (FQLNK) algorithm for solving trajectory optimization problems arising from aerospace industrial applications. As its name suggests, in this algorithm we first convert the constrained optimization problem into an unconstrained one by introducing the augmented Lagrangian parameters. The next step is to find the optimal candidate solution by solving the Karush-Kuhn-Tucker (KKT) system with a Newton-Krylov method. To reduce the computational cost of constructing the KKT system, we employ the Broyden-Fletcher-Goldfarb-Shanno (BFGS) formula to build an approximation of the (1,1) subblock of the KKT matrix, which is the most expensive part of the overall computation. The BFGS-based FQLNK algorithm exhibits a superior speedup compared to some of the alternatives. We demonstrate our FQLNK algorithm to be a practical approach for designing an optimal trajectory of a launch vehicle in space missions.

Trajectory Optimization Using Analytical Target Cascading

In the previous reports, analytical target cascading (ATC) is generally applied to product optimization. In this paper, the application area of ATC is expanded to trajectory optimization. Direct collocation method is utilized to convert a trajectory optimization into a nonlinear programing (NLP) problem. The converted NLP is a large-scale problem with sparse matrix of functional dependence table (FDT) suitable for the application of ATC. Three numerical case studies are provided to show the effects of ATC in solving trajectory optimization problems.

Education

This issue contains two articles in the Education section. The first one is “An Introduction to Trajectory Optimization: How to Do Your Own Direct Collocation,” by Matthew Kelly. The author presents a tutorial on discretization ideas for optimal control problems with ordinary differential equations. The article starts with a simple example, which is used to introduce some basic notions and a general formulation of an optimal control problem, where the dynamics is described by an ordinary differential equation. That formulation includes general state and control constraints. The main focus is put on methods, which construct a polynomial approximation of the state and control functions. The state and control spaces are discretized, and the infinite-dimensional control problem becomes a finite-dimensional nonlinear optimization problem. Piecewise linear and quadratic approximations of the state and the control are explained and illustrated by figures. Attention is brought to the error accumulated in the numerical calculation of integrals according to several methods. The author offers some practical suggestions for the initialization of the numerical optimization algorithm, as well as for the interpretation of the solution provided by the optimization method. The optimization models obtained as a result of the discretization procedure typically lack convexity properties; this entails that the optimization procedure would provide a local solution, which would not be globally optimal, in general. Some ideas about how to look for a better solution or confirm the quality of the one at hand are provided. Further discussion includes suggestions for smoothing of nondifferentiable functions that may occur in the problem and a comparison of open loop vs. closed loop solutions. The last section of the paper briefly surveys methods based on discretization and numerical treatment of the conditions of optimality. The author has made his software---a general-purpose MATLAB library for solving trajectory optimization problems, complete with documentation---publicly available at https://GitHub.com/MatthewPeterKelly/OptimTraj. The electronic supplement is described in Appendix A. In addition to the optimization software, it includes all example problems, which are presented in the paper. The second article is “How to Mitigate Sloshing,” authored by H. Ockendon and J. R. Ockendon. The authors start with an example from everyday life familiar to most of us: carrying a mug of coffee can often lead to spillage. How should we reduce this danger? The authors devise a model of this dynamical system under certain simplifying assumptions. A point (representing the hand carrying the coffee) is oscillating horizontally with a given frequency and amplitude (representing the walk) about a fixed mean position. A spring connects the hand to the coffee mug, which can slide freely on a smooth table. The effect of that spring on the motion of the liquid is the focus of attention. It is assumed additionally that the mug is rectangular and that no motion perpendicular to the direction of the action of the spring is present. The goal becomes to model and analyze the two-dimensional motion of liquid in a rectangular container of given width and depth. To this end, the authors introduce and analyze a mathematical model based on partial differential equations. Although the article is compact, the authors make a number of suggestions for future investigation and extensions.

Trajectory Optimization Based on Multi-Interval Mesh Refinement Method

In order to improve the optimization accuracy and convergence rate for trajectory optimization of the air-to-air missile, a multi-interval mesh refinement Radau pseudospectral method was introduced. This method made the mesh endpoints converge to the practical nonsmooth points and decreased the overall collocation points to improve convergence rate and computational efficiency. The trajectory was divided into four phases according to the working time of engine and handover of midcourse and terminal guidance, and then the optimization model was built. The multi-interval mesh refinement Radau pseudospectral method with different collocation points in each mesh interval was used to solve the trajectory optimization model. Moreover, this method was compared with traditional h method. Simulation results show that this method can decrease the dimensionality of nonlinear programming (NLP) problem and therefore improve the efficiency of pseudospectral methods for solving trajectory optimization problems.

A modified pseudospectral method for solving trajectory optimization problems with singular arc

This paper presents a direct method based on Legendre–Radau pseudospectral method for efficient and accurate solution of a class of singular optimal control problems. In this scheme, based on a priori knowledge of control, the problem is transformed to a multidomain formulation, in which the switching points appear as unknown parameters. Then, by utilizing Legendre‐Radau pseudospectral method, a nonlinear programming problem is derived which can be solved by the well‐developed parameter optimization algorithms. The main advantages of the present method are its superior accuracy and ability to capture the switching times. Accuracy and performance of the proposed method are examined by means of some numerical experiments. Copyright © 2016 John Wiley & Sons, Ltd.

Reentry Trajectory Optimization of Airbreathing Hypersonic Vehicles Based on Gauss Pseudospectral Method

According to the nonlinear, multivariable and multi-constraint features of the reentry trajectory optimization problem of airbreathing hypersonic vehicles, a suboptimal solution method is developed. The reentry trajectory generation is converted to a nonlinear programming (NLP) problem by using Gauss pseudospectral method (GPM). The state and control variables on Gauss nodes are chosen as parameters to be optimized and the minimum total heat absorption is chosen as the optimal performance index. Then the sequential quadratic programming (SQP) method is used to solve the NLP problem. The states of optimized trajectory are compared with the states obtained by the integral of kinetic equations. By simulating on an example of airbreathing hypersonic vehicles, it is demonstrated that the above method is not sensitive to the estimate of motion states and is easier to converge. And the method is effective to solve trajectory optimization problems.

Hermite-Legendre-Gauss-Lobatto Direct Transcription in Trajectory Optimization

D IRECT methods have been widely applied for solving trajectory optimization problems [1–5]. Among the popular methods are the Hermite–Simpson method [6] and the Legendre pseudospectral (PS)method [7,8]. There has been some considerable interest in developing theory related to the Legendre PS method due to its high accuracy, although the Hermite–Simpson method continues to be applied to large-scale practical problems [9]. The main characteristic of the Hermite–Simpson method is the combination of reasonable accuracy with a highly sparse constraint Jacobian and Hessian matrix [10]. The PS method offers impressive accuracy (spectral accuracy) for smooth problems, but the constraint Jacobians are much denser than other methods. The sparsity of the constraint Jacobians can be increased in the PS method by using knots [11]. In addition to the Hermite–Simpson method, additional highorder methods have been proposed by Herman and Conway [2]. These methods are attractive from the point of view of accuracy, but in the framework proposed by Herman and Conway, they require detailed derivation when extended to arbitrary higher orders. For instance, in [2], the form of the constraints was derived via the symbolic manipulation software MAPLE. The purpose of this Note is to provide an alternative framework for arbitrary higher-order methods suitable for implementation on digital computers and in a reusable form. The optimal control problem is approximated by a discrete nonlinear programming problem (NLP) by expanding the state trajectories using local Hermite interpolating polynomials. For high accuracy, the collocation points are selected from the family of Gauss–Lobatto points. This also allows the integral performance index to be approximated viaGauss– Lobatto quadrature rules. For optimal control problems of the Bolza form, the natural choice of quadrature points are the Legendre– Gauss–Lobatto (LGL) points, because they are derived on the basis of a unity weight function, giving the highest accuracy for polynomial integrands. The generalization of the approach is referred to as the Hermite–Legendre–Gauss–Lobatto (HLGL) approach throughout the remainder of this Note.

Trajectory Optimization Using Multiresolution Techniques

We present a multi-resolution-based approach for solving trajectory optimization problems. The original optimal control problem is solved using a direct method, thereby being transcribed into a nonlinear programming problem that is solved using standard nonlinear programming codes. The novelty of the proposed approach hinges on the automatic calculation of a suitable nonuniform grid over which the nonlinear programming problem is subsequently solved. This tends to increase numerical efficiency and robustness. Control and/or state constraints are handled with ease and without any additional computational complexity. The proposed algorithm is based on a simple and intuitive method to balance conflicting objectives, such as accuracy of the solution, convergence, and speed of computations. The benefits of the proposed algorithm over uniform grid implementations are demonstrated with the help of several nontrivial examples.

“Paradoxes” of optimal solutions in problems of space vehicle injection and reentry

Different types of local extremals obtained in solving trajectory optimization problems related to space vehicle injection and reentry by applying the Pontryagin’s maximum principle are demonstrated. Some solutions differ qualitatively from traditional ones. It is shown that almost each local extremal revealed provides a global optimum within a certain range of parameter values. The change in the type of the global extremal is, as a rule, of bifurcation nature. Multiplicity of local extremals and significantly nonlinear effects in the behavior of globally optimal solutions are basically caused by the presence of aerodynamic forces which can be sufficiently small as compared with the vehicle weight. Numerical results demonstrate the importance of the study of different types of local extremals at all aerospace vehicle design stages because the implementation of nontraditional optimal control laws can demand changes in the space vehicle configuration.

Flight Testing an Optimal Trajectory for the Saab J35 Draken

Introduction T HE desire to Ž nd the most efŽ cient  ight path has a long history.The developmentof the Ž rst high-performancejet aircraft made it crucial to  y a close to optimal path to perform a givenmission before the fuel ran out. The earliest methods for Ž nding optimal trajectorieswere based on calculusof variations.Discretization of the optimality conditions and solution using numerical methods made it possible to solve quite complicated problems; see Bryson and Desai for a discussion on early strategies. In recent years, the most common approach is to discretize the differential algebraic equations (DAEs) before stating optimality conditions and applying numericalmethods. This strategywas pioneeredbyHargravesandParis, who used theNPSOL optimization package.Recentdevelopmentsin discretizationofDAEs4 andsparse nonlinearoptimizationmethods i 8 represent the current state of the art in trajectory optimization. Althoughmany papers appear in the open literatureon the development of numerical methods for solving trajectory optimization problems, very few papers are available discussing the difŽ culties involved in trying to follow a numerically computed trajectory in a real  ight test. The purpose of the present paper is to report on a simple  ight test that was performed to demonstrate the difŽ culties that may appear in a  ight test. This paper describes the model and numerical implementation used to calculateoptimal trajectories.The presentationof numerical results then follows with the results obtained in the  ight test for a particular test case.

Global State Space Approach for the Efficient Numerical Solution of State-Constrained Trajectory Optimization Problems

A new approach based on a global state space form is introduced for solving trajectory optimization problems with state inequality constraints via indirect methods. The use of minimal coordinates on a boundary arc of the state constraint eliminates severe problems, which occur for standard methods and are due to the appearance of differential-algebraic boundary-value problems. Together with a hybrid approach and a careful treatment of some interior-point conditions, we obtain an efficient and reliable solution method.

Primal and dual formulations of sequential gradient-restoration algorithms for trajectory optimization problems

One of the most effective first-order algorithms for solving trajectory optimization problems is the sequential gradient-restoration algorithm (SGRA). Originally developed in the primal formulation (PSGRA), this algorithm is extended to incorporate a dual formulation (DSGRA). Both the primal formulation and the dual formulation involve a sequence of two-phase cycles, each cycle including a gradient phase and a restoration phase. In turn, each iteration of the gradient phase and the restoration phase requires the solution of an auxiliary minimization problem (AMP). In the primal formulation, the AMP is solved with respect to the variations of the state, the control and the parameter. In the dual formulation, the AMP is solved with respect to the Lagrange multipliers. A characteristic of the dual formulation is that the AMP's associated with the gradient phase and the restoration phase of SGRA can be reduced to mathematical programming problems involving a finite number of parameters as unknowns. A comparison of the primal formulation and the dual formulation is presented. The comparison is done in terms of several trajectory optimization problems having current aerospace interest.

A combined parallel-tangents/penalty-function approach to solving trajectory optimization problems

A combined parallel-tangents/penalty-function approach to solving trajectory-optimization problems with terminal state constraints is discussed. The procedure has been designed to give accelerated convergence when using penalty-functions to handle terminal constraints. An attractive feature of the procedure is that its implementation is only slightly more complicated than the standard steepest-descent procedure. An example is presented illustrating the improved convergence properties afforded by this procedure.

An improved analogue technique for solving trajectory optimization problems†

Abstract Improvements are made to the basic analogue -technique and experimental results obtained from three linear systems. A method is described for modifying the objective function contours to allow overall convergence of the iteration. Experimental results are obtained with the modified contours and compared with the results given by the basic technique.

An analogue technique for solving trajectory optimization problems

Analogue or hybrid computer methods for solving trajectory optimization problems usually require the solution of a two-point boundary value problem. A method of solving this problem is presented which does not require a good initial approximation to ensure convergence of the iteration. Analogue computer results are given with the method applied to a double integrator plant. Two different forms of performance index are considered.

A modified Newton's method for solving trajectory optimization problems.

Modified Newton method for solution of simultaneous nonlinear equations applied to variational two point boundary value problems in trajectory optimization