Preprint Convex Optimization for Trajectory Generation A Tutorial On Generating Dynamically Feasible Trajectories Reliably And Efficiently Danylo Malyuta a,? , Taylor P. Reynolds a, Michael Szmuk a, Thomas Lew b, Riccardo Bonalli b, Marco Pavone b, and Behc¸et Ac¸ıkmes¸e a a William E. Boeing Department of Aeronautics and Astronautics, University of Washington, Seattle, WA 98195, USA b Department of Aeronautics and Astronautics, Stanford University, Stanford, CA 94305, USA Reliable and efficient trajectory generation methods are a fundamental need for autonomous dynamical systems of tomorrow. The goal of this article is to provide a comprehensive tutorial of three major con- vex optimization-based trajectory generation methods: lossless convexification (LCvx), and two sequential convex programming algorithms known as SCvx and GuSTO. In this article, trajectory generation is the computation of a dynamically feasible state and control signal that satisfies a set of constraints while opti- mizing key mission objectives. The trajectory generation problem is almost always nonconvex, which typ- ically means that it is not readily amenable to efficient and reliable solution onboard an autonomous ve- hicle. The three algorithms that we discuss use problem reformulation and a systematic algorithmic strat- egy to nonetheless solve nonconvex trajectory generation tasks through the use of a convex optimizer. The theoretical guarantees and computational speed offered by convex optimization have made the algo- rithms popular in both research and industry circles. To date, the list of applications include rocket land- ing, spacecraft hypersonic reentry, spacecraft rendezvous and docking, aerial motion planning for fixed- wing and quadrotor vehicles, robot motion planning, and more. Among these applications are high-profile rocket flights conducted by organizations like NASA, Masten Space Systems, SpaceX, and Blue Origin. This article aims to give the reader the tools and understanding necessary to work with each algorithm, and to know what each method can and cannot do. A publicly available source code repository supports the numerical examples provided at the end of this article. By the end of the article, the reader should be ready to use each method, to extend them, and to contribute to their many exciting modern applications. utonomous vehicles and robots promise many ual vehicles must be endowed with their own high qual- exciting new applications that will transform ity decision making capability. Failure to generate a safe our society. For example, autonomous aerial trajectory can result in losing the vehicle, the payload, vehicles (AAVs) operating in urban envi- and even human life. Reliable methods for trajectory ronments could deliver commercial goods, generation are a fundamental need if we are to maintain Aemergency medical supplies, monitor traffic, and provide public trust and the high safety standard that we have threat alerts for national security [1]. At the same come to expect from autonomous or automatic systems time, these applications present significant engineering [5]. challenges for performance, trustworthiness, and safety. Computational resource requirements are the second For instance, AAVs can be a catastrophic safety hazard major consideration for onboard trajectory generation. arXiv:2106.09125v1 [math.OC] 16 Jun 2021 should they lose control or situational awareness over a Historically, this has been the driving factor for much populated area. Space missions, self-driving cars, and of practical algorithm development [6,7]. Although applications of autonomous underwater vehicles (AUVs) the modern consumer desktop is an incredibly powerful share similar concerns. machine, industrial central processing units (CPUs) can Generating a trajectory autonomously onboard the ve- still be relatively modest [8]. This is especially true hicle is not only desirable for many of these applications for spaceflight, where the harsh radiation environment but also indeed a necessity when considering the deploy- of outer space prevents the rapid adoption of new ment of autonomous systems either in remote areas with computing technologies. For example, NASA's flagship little to no communication, or at scale in a dynamic and Mars rover \Perseverance" landed in 2021 using a uncertain world. For example, it is not possible to re- BAE RAD750 PowerPC flight computer, which is a motely control a spacecraft during a Mars landing sce- 20 year-old technology [9, 10]. When we factor in the nario [2,3,4], nor is it practical to coordinate the motion energy requirements of powerful CPUs and the fact that of tens of thousands of delivery drones from a centralized trajectory generation is a small part of all the tasks location [1]. In these and many other scenarios, individ- that an autonomous vehicle must perform, it becomes ? Corresponding author. clear that modern trajectory generation is still confined E-mail address: [email protected] to a small computational footprint. Consequently, real- 1 Preprint time onboard trajectory generation algorithms must be The cost (1a) encodes the mission goal, the system computationally efficient. dynamics are modeled by the differential equation con- We define trajectory generation to be the computation straint (1b), the state and control specifications are en- of a multi-dimensional temporal state and control signal forced through (1c), and the boundary conditions are that satisfies a set of specifications, while optimizing key fixed by (1d). Note that Problem1 makes a distinction mission objectives. This article is concerned exclusively between a control vector u(t), which is a temporal signal, with dynamically feasible trajectories, which are those and a so-called parameter vector p, which is a static vari- that respect the equations of motion of the vehicle under able that encodes other decision variables like temporal consideration. Although it is commonplace to track dy- scaling. namically infeasible trajectories using feedback control, In most cases, the solution of such an infinite-dimen- at the end of the day a system can only evolve along sional optimization problem is neither available in closed dynamically feasible paths, whether those are computed form nor is it computationally tractable to numerically upfront during trajectory generation or are the result compute (especially in real-time). Instead, different so- of feedback tracking. Performing dynamically feasible lution methods have been proposed that typically share trajectory generation carries two important advantages. the following three main components: First, it provides a method to systematically satisfy con- straints (i.e., specifications) that are hard, if not impos- Formulation: specification of how the functions J and f, and the sets , 0, and f are expressed mathe- sible, to meet through feedback control. This includes, C X X for example, translation-attitude coupled sensor pointing matically; constraints. Secondly, dynamically feasible trajectories Discretization: approximation of the infinite-dimen- leave much less tracking error for feedback controllers to sional state and control signal by a finite-dimensional \clean up", which usually means that tracking perfor- set of basis functions; mance can be vastly improved. These two advantages shall become more apparent throughout the article. Numerical optimization: iterative computation of an Numerical optimization provides a systematic mathe- optimal solution of the discretized problem. matical framework to specify mission objectives as costs or rewards to be optimized, and to enforce state and Choosing the most suitable combination of these three control specifications as well as the equations of motion components is truly a mathematical art form that is via constraints. As a result, we can express trajectory highly problem dependent, and not an established plug- generation problems as optimal control problems, which and-play process like least-squares regression [26]. No are infinite-dimensional optimization problems over func- single recipe or method is the best, and methods that tion spaces [11, 12]. Since the early 1960s, optimal con- work well for some problems can fare much worse for trol theory has proven to be extremely powerful [6, 13]. others. Trajectory algorithm design is replete with prob- Early developments were driven by aerospace applica- lem dependent tradeoffs in performance, optimality, and tions, where every gram of mass matters and trajectories robustness, among others. Still, optimization literature are typically sought to minimize fuel or some other mass- (to which this article belongs) attempts to provide some reducing metric (such as aerodynamic load and thereby formal guidance and intuition about the process. For structural mass). This led to work on trajectory algo- the rest of this article, we will be interested in meth- rithms for climbing aircraft, ascending and landing rock- ods that solve the exact form of Problem1, subject ets, and spacecraft orbit transfer, to name just a few only to the initial approximation made by discretizing [14, 15, 16, 17, 18, 19]. Following these early applica- the continuous-time dynamics. tions, trajectory optimization problems have now been Many excellent references discuss the discretization formulated in many practical areas, including aerial, un- component [13, 27, 28]. Once the problem is discretized, derwater, and space vehicles, as well as for chemical pro- we can employ numerical optimization methods to obtain cesses [20, 21, 22], building