AN INTRODUCTION TO TRAJECTORY OPTIMIZATION: HOW TO DO YOUR OWN DIRECT COLLOCATION ∗ MATTHEW KELLY
Abstract. This paper is an introductory tutorial for numerical trajectory optimization with a focus on direct collocation methods. These methods are relatively simple to understand and effectively solve a wide variety of trajectory optimization problems. Throughout the paper we illustrate each new set of concepts by working through a sequence of four example problems. We start by using trapezoidal collocation to solve a simple one-dimensional toy-problem and work up to using Hermite–Simpson collocation to compute the optimal gait for a bipedal walking robot. Along the way, we cover basic debugging strategies and guidelines for posing well-behaved optimization problems. The paper concludes with a short overview of other methods for trajectory optimization. We also provide an electronic supplement that contains well-documented Matlab code for all examples and methods presented in this paper. Our primary goal is to provide the reader with the resources necessary to understand and successfully implement their own direct collocation methods.
1. Introduction. What is trajectory optimization? Let’s start with an example: imagine a satellite moving between two planets. We would use the term trajectory to describe the path the the satellite takes between the two planets. Usually, this path would include both state (e.g. position and velocity) and control (e.g. thrust) as functions of time. The term trajectory optimization refers to a set of methods that are used to find the best choice of trajectory, typically by selecting the inputs to the system, known as controls, as functions of time. 1.1. Overview. Why read this paper? Our contribution is to provide a tutorial that covers all of the basics required to understand and implement direct collocation methods, while still being accessible to broad audience. Where possible, we teach through examples, both in this paper and in the electronic supplement. This tutorial starts with a brief introduction to the basics of trajectory optimization (§1), and then it moves on to solve a simple example problem using trapezoidal collocation (§2). The next sections cover the general implementation details for trapezoidal collocation (§3) and Hermite–Simpson collocation (§4), followed by a section about practical implementation details and debugging (§5). Next there are three example problems: cart-pole swing-up (§6), five-link bipedal walking (§7), and minimum-work block-move (§8). The paper concludes with an overview of related optimization topics and a summary of commonly used software packages (§9). This paper comes with a two-part electronic supplement, which is described in detail in the appendix §A. The first part is a general purpose trajectory optimization library, written in Matlab, that implements both trapezoidal direct collocation, Hermite–Simpson direct collocation, direct multiple shooting (4th -order Runge–Kutta), and global orthogonal collocation (Chebyshev Lobatto). The second part of the supplement is a set of all example problems from this paper implemented in Matlab and solved with the afore-mentioned trajectory optimization library. The code in the supplement is well-documented and designed to be read in a tutorial fashion. 1.2. Notation. For reference, these are the main symbols we will use throughout the tutorial and will be described in detail later. tk N
time at knot point k number of trajectory (spline) segments
hk = tk+1 − tk xk = x(tk )
duration of spline segment k state at knot point k
uk = u(tk ) wk = w tk , xk , uk fk = f tk , xk , uk q˙ =
d dt q
control at knot point k integrand of objective function at knot point k system dynamics at knot point k 2
d dt2 q
first and second time-derivatives of q
work was supported by the National Science Foundation University, Ithaca, NY.