THE UNIVERSITY of QUEENSLAND Bachelor of Engineering Thesis

THE UNIVERSITY OF QUEENSLAND

Bachelor of Engineering Thesis

Development of Multiple-Phase Trajectory Planner Using Legendre-Gauss-Radau Collocation Methods

Student Name: Inderpreet METLA

Course Code: MECH4500

Supervisor: Dr Michael Kearney

Submission Date: 25 October 2018

A thesis submitted in partial fulfilment of the requirements of the Bachelor of Engineering degree in Mechanical Engineering

UQ Engineering

Faculty of Engineering, Architecture and Information Technology

Abstract

Trajectory planning is a technique used to solve optimisation problems for constrained dynamic systems. Advances in personal computers have enabled trajectory planning techniques to become prevalent within many branches of engineering, such as the aerospace and robotics industries. However, practical problems within these industries can be complex and intricate. In particular, such complex problems can exhibit significant changes in the dynamics, constraints and performance measures as systems evolve over time. A technique used to handle these problems is known as “multiple-phase” trajectory planning. In this technique, the time domain is partitioned into connected phases such that each phase can be formulated with a distinct set of parameters and equations. At present, there are no open-source packages available for array-oriented programming languages that can solve multiple-phase trajectory planning problems. As a result, a scope has been identified to develop such a software. Within this thesis, a MATLAB implementation of a multiple-phase trajectory planning software called the Pseudospectral OPTimiser (풫풪풫풯) is developed. The software uses a variable-order Legendre-Gauss-Radau (LGR) orthogonal collocation technique to transcribe the continuous time trajectory planning problem into a sparse nonlinear program (NLP). In this technique, solution trajectories are approximated using orthogonal polynomials with the system dynamics and constraints collocated at LGR quadrature points. The resulting NLP subproblem is solved by employing an interior-point optimisation method. A grid refinement algorithm has been implemented that can determine the degree of the approximating polynomial, as well as the number of grid intervals required to achieve a specified level of accuracy. The software efficiently calculates the first and second order derivatives required by the NLP solver by exploiting the well-defined Jacobian and Hessian sparsity patterns of the LGR pseudospectral method. The performance of the software has been evaluated on five benchmark problems from literature and then compared against a variety of open-source and commercial packages. The problems range in complexity from single-phase two-point boundary problems to multiple- phase problems with numerous constraints and non-convex performance measures. The results show that 풫풪풫풯 has superior performance compared to many open-source counterparts in terms of computation time, optimality and solution accuracy. 풫풪풫풯 has also demonstrated comparable performance against commercial solvers. The outcomes of this thesis prove that 풫풪풫풯 is a highly competitive open-source software that can solve a broad range of optimal control problems, and will be a valuable addition to the existing literature.

iii

Acknowledgements

First and foremost, I would like to thank my supervisor, Dr Michael Kearney, for providing me guidance, support and direction throughout this thesis. This has been the most stimulating work I have done throughout my degree, and I would like to thank Michael for allowing me to change the direction of my thesis so I could study this topic. I would also like to thank Michael for taking an active interest in me outside of my studies and always being willing to chat about other aspects of life.

I would like to acknowledge and thank Sholto Forbes-Spyratos for allowing me to borrow his GPOPS-II license so I could benchmark my solver. I would also like to thank Matthew Kelly from Cornell University for providing a stranger some assistance and answering my questions.

To my friends who have supported and pushed me all throughout my degree, I thank you. I wish you all the best of luck in your next chapter.

I would like to thank my family for always providing me their love and support towards my decisions and goals throughout my studies.

Lastly, to Emily, who has provided me love and unwavering support throughout this year, I thank you.

Table of Contents

Abstract ...... iii

List of Figures ...... ix

List of Tables ...... xi

1 Introduction ...... 1

1.1 Project Motivation and Purpose ...... 1

1.2 Project Goals and Intended Outcomes ...... 3 1.3 Project Scope ...... 4 1.3.1 In Scope ...... 4 1.3.2 Out of Scope ...... 4 1.4 Thesis Outline ...... 5

2 Literature Review ...... 6

2.1 Optimal Control History and Solution Approach ...... 6 2.2 Methods to Formulate Optimal Control Problems ...... 8 2.3 General Formulation of a Trajectory Optimisation Problem ...... 9 2.4 Survey of Numerical Methods to Solve Trajectory Optimisation Problems . . 10

2.4.1 Dynamic Programming ...... 10 2.4.2 Differential Dynamic Programming ...... 10 2.4.3 Indirect and Direct Transcription Methods ...... 11

2.4.4 Direct Single Shooting ...... 12

2.4.5 Direct Multiple Shooting ...... 12

2.4.6 Direct Collocation and Local Orthogonal Collocation ...... 13

2.4.7 Pseudospectral Collocation (Global Orthogonal Collocation) ...... 14 2.5 Nonlinear Programming ...... 17

2.5.1 Formal Definitions ...... 17

2.5.2 Algorithms to Solve NLPs ...... 18

2.5.2.1 Sequential Quadratic Programming ...... 18

2.5.2.2 Interior Point Methods ...... 19

2.6 Derivative Calculation Methods ...... 20 2.6.1 Finite-Differencing ...... 21 2.6.2 Complex-Step Differentiation ...... 21 2.6.3 Automatic Differentiation ...... 22

2.7 Grid Refinement ...... 22

2.8 Techniques to Scale Optimal Control Problems ...... 25

2.9 Available Trajectory Optimisation Software ...... 26

2.10 Chapter Summary ...... 27

3 Required Components for Software and Design

Decisions ...... 28

3.1 Overview of Planned Components ...... 28 3.1.1 Influence on Software Structure from Trajectory Optimisation

Problem ...... 28

3.1.2 Choice of Numerical Transcription Method ...... 29

3.1.3 Choice of NLP Solver ...... 31

3.1.4 Choice of Derivative Calculation Methods ...... 32

3.1.5 Problem Scaling Approach ...... 33

3.1.6 Choice of Grid Refinement Approach ...... 33

4 Mathematical Development of Method ...... 34

4.1 Formulation of General Multiple-Phase Trajectory Optimisation Problem . . 34 4.2 Formulation of Multiple-Interval, Multiple-Phase Radau Pseudospectral

Method ...... 35

4.2.1 Multiple-Interval Formulation of Objective and Constraints ...... 35

4.2.2 State Approximation using the RPM ...... 39

4.2.3 Control Approximation using the RPM ...... 40

4.2.4 Total Nonlinear Problem Discretisation using the RPM ...... 40 4.3 Conversion of Radau Pseudospectral Method Formulation into NLP

Algorithms ...... 42 4.3.1 Construction of Decision Variable Vector and its Upper and Lower

Bounds ...... 42

4.3.2 Construction of Constraint Vector and its Upper and Lower Bounds . . 43

4.4 Chapter Summary ...... 45

5 Extensions to Software for Tractability 46

5.1 Grid Refinement Algorithm ...... 46

5.2 Exploiting Sparsity in the Radau Pseudospectral Method ...... 50

5.2.1 Sparsity Patterns of the RPM Constraint Jacobian and Lagrangian

Hessian ...... 50

5.2.2 Exploiting Sparsity to Compute Derivatives ...... 54

6 Software Architecture ...... 57

7 Usage of Software and Constructing a Problem ...... 59

7.1 Constructing a Problem ...... 59

7.1.1 Creating the problem.funcs Struct ...... 62

7.1.2 Syntax for Dynamics, Path Objective and Path Constraint Functions . . 62

7.1.3 Syntax for Boundary Objective and Boundary Constraint Functions . . 63

7.1.4 Creating the problem.bounds Struct ...... 64

7.1.5 Creating the problem.guess Struct ...... 65

7.2 Details for the Output of the Software ...... 66

8 Results: Implementation and Performance Evaluation on

Example Problems ...... 67

8.1 Overview of Example Problems ...... 67

8.2 Example 1 – Continuous Time Infinite-Horizon LQR ...... 68

8.3 Example 2 – Bryson Denham ...... 71

8.3.1 Comparison of 풫풪풫풯 and 픾ℙ핆ℙ핊 − 핀핀 ...... 71

vii

8.3.2 Comparison of 풫풪풫풯 and OptimTraj ...... 73

8.4 Example 3 – Free-Flying Robot ...... 74

8.5 Example 4 – Lee-Ramirez Bioreactor ...... 78

8.6 Example 5 – Goddard Rocket ...... 83

8.7 Chapter Summary ...... 87

9 Haul Truck Energy Management Case Study (HTEMCS) . . 88

9.1 Review of Optimal Energy Management Strategies ...... 88

9.2 General Problem Setup for the HTEMCS ...... 91

9.3 Multi-Phase Trajectory Optimisation Formulation for the HTEMCS ...... 94

9.4 Results of the HTEMCS ...... 96

10 Limitations of the Software ...... 100

11 Conclusions and Recommendations for Future Work . . . . 101

11.1 Conclusions and Summary of Outcomes ...... 101

11.2 Recommendations for Future Work ...... 103

References ...... 104

Appendix A: 퓟퓞퓟퓣 User’s Guide ...... 114

Appendix B: 퓟퓞퓟퓣 Code for Example 1 ...... 126

Appendix C: 퓟퓞퓟퓣 Code for Example 2 ...... 128

Appendix D: 퓟퓞퓟퓣 Code for Example 3 ...... 130

Appendix E: 퓟퓞퓟퓣 Code for Example 4 ...... 132

Appendix F: 퓟퓞퓟퓣 Code for Example 5 ...... 134

Appendix G: 퓟퓞퓟퓣 Code for HTEMCS ...... 137

viii

List of Figures

Figure 2.1: Optimal control problem direct transcription process ...... 7

Figure 2.2: Optimal control problem solution techniques ...... 8

Figure 2.3: Single shooting vs Multiple shooting ...... 13

Figure 2.4: Comparison between h- and p-Methods ...... 14

Figure 2.5: Location of LG, LGR and LGL points ...... 15 Figure 2.6: Flow diagram for GPM’s equivalent optimality conditions for Direct and 16

Indirect approaches ......

Figure 2.7: h- and p- methods of grid refinement ...... 23

Figure 3.1: Decision tree for choice of transcription method ...... 29 Figure 3.2: Example of a multi-interval, multi-phase problem using the RPM...... 30

Figure 4.1: Example of a two-interval formulation for the state trajectory ...... 38

Figure 5.1: Single-phase constraint Jacobian sparsity pattern for the RPM ...... 51

Figure 5.2: Multiple-phase constraint Jacobian sparsity pattern for the RPM ...... 52

Figure 5.3: Single-phase Lagrangian Hessian sparsity pattern for the RPM ...... 53

Figure 5.4: Multiple-phase Lagrangian Hessian sparsity pattern for the RPM ...... 53

Figure 6.1: Flowchart of Pseudospectral OPTimiser (풫풪풫풯) ...... 57

Figure 8.1: Comparison between the solution for the Infinite-Horizon LQR problem

from 풫풪풫풯 and the exact solution on the closed domain [-1,1] ...... 70 Figure 8.2: Comparison between the solution for the Infinite-Horizon LQR problem from 풫풪풫풯 and the exact solution on the semi-infinite time domain . . . . . 70 Figure 8.3: Comparison of solution for the Bryson-Denham problem from 풫풪풫풯 and

the exact solution ...... 72

Figure 8.4: Comparison of solution for the Bryson-Denham problem from 풫풪풫풯 and 73

OptimTraj ......

Figure 8.5: State trajectory for Free-Flying Robot problem from 풫풪풫풯 ...... 76

Figure 8.6: Control trajectory for Free-Flying Robot problem from 풫풪풫풯 ...... 76 Figure 8.7: Distribution of nodes after grid refinement for the control input in the Free-

Flying Robot problem ...... 77

Figure 8.8: State solution for Lee-Ramirez Bioreactor problem obtained by 풫풪풫풯 . . . 81

Figure 8.9: Control solution for Lee-Ramirez Bioreactor problem obtained by 풫풪풫풯 . . 81

Figure 8.10: Control solution for Lee-Ramirez Bioreactor problem obtained by 풫풮풪풫풯. 82 Figure 8.11: Control solution for a single-phase formulation of the Goddard Rocket

problem ...... 84 Figure 8.12: State and control solution for the three-phase formulation of the Goddard Rocket problem ...... 86

Figure 9.1: Drive system configuration for a DEMHT with the energy recovery system

installed ...... 91

Figure 9.2: Rim-pull and dynamic brake force available at various velocities ...... 92

Figure 9.3: State solution for HTEMCS – Min Time and Min Fuel Formulation . . . . . 97

Figure 9.4: Control solution for HTEMCS – Min Time and Min Fuel Formulation . . . . 97

Figure 9.5: State solution for HTEMCS – Min Time Formulation ...... 99

Figure 9.6: Control solution for HTEMCS – Min Time Formulation ...... 99

List of Tables

Table 2.1: Existing software packages ...... 26

Table 3.1: Summary of a comparison of IP and SQP solvers by Mittelmann (2018) . . . 31

Table 5.1: Comparison between two approaches to compute Jacobian and Hessian

matrices ...... 55

Table 7.1: Options and default settings for problem.derivatives ...... 61

Table 7.2: Options and default settings for problem.grid ...... 61

Table 7.3: Options and default settings for problem.options ...... 61

Table 8.1: Example problems and their features ...... 67

Table 8.2: Summary of execution of 풫풪풫풯 for Infinite-Horizon LQR problem . . . . . 69

Table 8.3: Comparison of solution from 풫풪풫풯 and 픾ℙ핆ℙ핊 − 핀핀 ...... 72 Table 8.4: Comparison of solution procedure from 풫풪풫풯, 픾ℙ핆ℙ핊 − 핀핀 and 핊핆ℂ핊 for

Free-Flying Robot problem ...... 76

Table 8.5: Comparison of solution procedure from 풫풪풫풯, 풫풮풪풫풯 and PROPT for the

Lee-Ramirez Bioreactor problem ...... 80

Table 8.6: Comparison of solution from 풫풪풫풯 and 픾ℙ핆ℙ핊 − 핀핀 for the Goddard

Rocket problem ...... 85

Table 9.1: Haul route definition ...... 93

Table 9.2: Summary of solution procedure for HTEMCS ...... 96

Table 11.1: Recommendations for future work for 풫풪풫풯 ...... 103

Table A.1: Options and default settings for problem.derivatives ...... 118

Table A.2: Options and default settings for problem.grid ...... 119

Table A.3: Options and default settings for problem.options ...... 119

THIS PAGE INTENTIONALLY LEFT BLANK

xii

Chapter 1 Introduction

1.1 Project Motivation and Purpose

Trajectory planning, or trajectory optimisation, refers to a class of techniques that are used within the optimisation of dynamic systems (Diehl and Gros, 2017). A dynamic system is characterised by states, which allow for the dynamic behaviour of the system to be predicted as it evolves over time, and controls, which are the inputs used to control the system. The sequence of control inputs is chosen in order to optimise some objective function, subject to any constraints on the system’s motion (Rao, 2009).

As an example of a trajectory planning problem, consider a train where the state vector consists of the position and velocity, and the control is the engine power selected by the driver. An objective could be to minimise the energy consumption as the train travels from Station A to Station B, and a potential constraint could be the time at which the train is to arrive at Station B (Diehl and Gros, 2017).

The use of numerical methods is essential in solving trajectory planning problems. Since the advent of the personal computer, optimisation problems posed as trajectory planning problems have become increasingly prevalent in many branches of engineering, such as the aerospace, robotics, manufacturing and chemical process industries (Betts, 2010). Optimal control methods can be used to solve many of these problems; however, for complex and intricate problems these methods are not straightforward to implement efficiently. Such complex problems can be categorised as having significant, or discontinuous, changes in the dynamics, constraints or objective functions as they evolve over time. These problems need to be partitioned along the time domain into connected phases when the significant changes occur. These are known as “multiple-phase” trajectory optimisation problems (Betts, 2010).

Although commercial packages are available that can handle multiple-phase problems, the availability of tractable open-source software for array-oriented programming languages, such as MATLAB, is scarce (Patterson and Rao, 2012b). This limits a user’s ability to learn the underlying mathematics and methodologies that the software packages implement. Hence, the purpose of this thesis is to create a MATLAB implementation of a multiple-phase trajectory 1

planner that simultaneously provides users an opportunity to learn the underlying methodology and, if need be, freely adapt the software to fit their needs. The source code will be made available on GitHub under the MIT License for open-source software (Open Source Initiative, 2006).

To motivate the necessity of multiple-phase solvers, consider the example of a space vehicle undergoing an aero-assisted orbital transfer from geostationary to low Earth orbit, as presented in Rao et al. (2002). During the transfer, the mass of the vehicle and the differential equations modelling the vehicle’s dynamics change significantly, as there are segments of the trajectory exposed to atmospheric conditions and other segments exposed to exoatmospheric conditions. A single-phase solver would not be able to handle such changes. However, within a multiple- phase solver, each segment of the transfer can be formulated as a distinct phase. This formulation allows for the changes in parameters and equations to be modelled.

A second contributor to the project’s motivation is a case study that has come about due to work conducted by Esfahanian (2014) and Terblanche et al. (2018) on the implementation of energy storage systems (ESS) on-board Diesel-Electric drive mining haul trucks (DEMHTs). These trucks are a parallel hybrid electric vehicle (HEV) that are currently used within the mining industry to help reduce fuel usage and emissions (Esfahanian, 2014). They can complete nearly 60 haul cycles per day and transport payloads weighing over 300 t (Liebherr, 2014). In doing so, over 1.5 ML of fuel can be consumed by a truck per year. When a typical 250 t truck, such as the Caterpillar 795F AC (Caterpillar, 2013), descends into a 100 m deep open-pit mine, it dispels approximately 245 MJ of potential energy via its resistor banks (Terblanche et al., 2018). However, these trucks can reverse the operation of their electric traction motors to function as generators and recover energy whilst braking – a concept commonly referred to as regenerative braking. This gives rise to the opportunity for an ESS to be installed that can store energy extracted by the regenerative brake. This energy can then be reinjected to provide propulsion when it is most beneficial, in order to improve fuel efficiency and productivity (Esfahanian, 2014).

The above can be transcribed into the following energy management optimal control problem:

Construct a mathematical model of a DEMHT that travels along a pre-defined haul route and then optimise the use of an ESS to minimise fuel consumption, cycle time or a combination of the two, subject to operational constraints.

Within this document the above problem will be referred to as the ‘Haul Truck Energy Management Case Study’ (HTEMCS). This problem can either be approached using closed- loop techniques, such as Dynamic Programming, or it can be approached using multiple-phase trajectory optimisation. Using trajectory optimisation over Dynamic Programming is appealing as the latter suffers from a concept known as the “curse of dimensionality”, causing the method to become computationally intractable as problems grow (Betts, 1998). Since the trajectory optimisation approach will require using multiple-phase techniques, and as there is a lack of open-source software available that can accommodate such techniques, the HTEMCS motivates the development of the multiple-phase trajectory planning software.

1.2 Project Goals and Intended Outcomes

The goals and intended outcomes of this thesis are defined below and will serve to direct the project.

1. Develop a software that can handle multiple-phase trajectory optimisation problems and solve these using direct or indirect transcription methods*. 2. Test and validate the program on problems with known solutions from literature and critically compare its performance against other available solvers. 3. Develop the software to be as computationally efficient as possible. 4. Use the software to solve the HTEMCS and evaluate the results.

* Transcription is the process of converting a continuous time problem into a discrete form and then solving the discrete subproblem. 3

1.3 Project Scope

At the heart of this project is the development of the multiple-phase trajectory planning solver. Although in-depth analysis of the HTEMCS is implied, the focus is heavily upon the development of the solver. As such, the scope of the project is defined as follows.

1.3.1 In Scope

 Critical review of solution techniques and available software to form the requirements for the solver.  Development of mathematical formulations for the general multiple-interval, multiple- phase trajectory optimisation problem* and the selected transcription technique.  Development of a software in MATLAB that applies the selected transcription technique to solve multiple-interval, multiple-phase trajectory optimisation problems and solely incorporates open-source toolboxes as part of its implementation.  Performance evaluation of the developed software on benchmark problems with analytical solutions, as well as problems that have been studied extensively in literature.  A user’s guide detailing how the software is used (in Appendix A).  Review of approaches in literature used to solve problems similar to the HTEMCS.  Formulation of mathematical models representing the DEMHT and ESS.  Choice of haul route characteristics for the HTEMCS.  Development of operational conditions and constraints for the HTEMCS.

1.3.2 Out of Scope

 Compatibility of the software with more than one programming language.  The ability for the software to solve multiple-phase problems where the phases are not connected sequentially. The most general formulation of multiple-phase problems does not necessitate that phases be connected sequentially; however, a non-sequential approach adds significant complexity to the software and these types of problems are uncommon (Betts, 2010).  Steering control of the vehicle in the HTEMCS.  Design and selection of the ESS.

* Multiple-interval, multiple-phase refers to a problem that is modelled using one or more phases and solved on a grid containing one or more intervals. 4

1.4 Thesis Outline

The remainder of this thesis has been structured as follows.

 Chapter 2 presents a comprehensive review of literature surrounding methods to solve trajectory optimisation problems and various transcription techniques. Background regarding differentiation techniques and grid refinement algorithms is also presented.  Chapter 3 presents the components planned for the solver and then outlines methods and design decisions regarding the implementation of these components.  Chapter 4 presents the mathematical formulation of a general continuous time multiple- phase trajectory optimisation problem. This is then extended to also include a multiple- interval formulation. Lastly, the mathematics to transcribe the resulting multiple- interval, multiple-phase formulation into a discrete nonlinear program* is outlined.  Chapter 5 presents two extensions to the software that allow it to be differentiated from many other available open-source packages. The two extensions are the development of a grid refinement algorithm, as well as an efficient method to compute derivatives for the nonlinear program by exploiting Jacobian and Hessian sparsity patterns. These extensions are designed to assist with improving solution accuracy and providing computational performance benefits to the software.  Chapter 6 outlines the entire architecture of the developed software with the use of a flow diagram.  Chapter 7 details how problems are set-up and interfaced with the software by a user. The functions required to fully define a problem are outlined within this chapter, as is the syntax of the software.  Chapter 8 presents the results of a benchmarking analysis where the software is compared against a variety of commercial and open-source solvers on five example optimal control problems from literature.  Chapter 9 encompasses the formulation, solution and subsequent results and analysis of the HTEMCS.  Chapter 10 describes the limitations of the developed software. Potential issues and restrictions that can arise due to certain in-built MATLAB functions are also outlined.  Chapter 11 concludes the thesis and provides recommendations for future work.

* A nonlinear program is the discrete subproblem that will be solved by the software in place of the continuous time trajectory optimisation formulation. A discussion into nonlinear programming is provided in Section 2.5. 5

Chapter 2 Literature Review

Prior to outlining the development of the trajectory planning software, a review of the relevant literature is presented. This review provides a brief history of optimal control and then outlines the various techniques employed to solve problems in this field. In particular, an in-depth discussion is presented regarding the so-called direct transcription techniques. The nonlinear program (NLP) subproblem that results from direct transcription is then formally defined. Optimisation algorithms to solve NLPS are subsequently explored along with various differentiation techniques. Methods to improve solutions, such as grid refinement and problem scaling, are then presented. Lastly, a summary of currently available software to solve optimal control problems across a variety of programming languages is presented and potential voids left by these programs are identified.

2.1 Optimal Control History and Solution Algorithm

Optimal control theory builds upon the calculus of variations, which has its origins throughout the 17th, 18th and 19th centuries, deriving from work conducted by Leonard Euler, Joseph-Louis Lagrange, Adrien-Marie Legendre, Carl Gustav Jacob Jacobi, Sir William Hamilton and Karl Weierstrass (Bryson, 1996). Throughout the 20th century, further rigorous analysis was done on the subject by Richard Bellman who developed a generalised approach to the Hamilton-Jacobi theory known as Dynamic Programming (Bryson, 1996). Subsequent work was also done by the likes of Edward McShane and Lev Pontryagin (Bryson, 1996), with the latter developing the well-known Pontryagin’s Maximum Principle (PMP), (Pontryagin, 1962).

Optimal control problems involve continuous functions of system dynamics, controls and constraints. They can be regarded as being infinite-dimensional extensions of finite- dimensional NLPs, which are categorised by a finite set of variables and constraints (Betts, 2010). The term “finite-dimensional” refers to a problem where a decision variable, say 퐱, can be represented with finitely many numbers, i.e. 퐱 ∈ℝ, where 푛 ∈ℕ. Otherwise, the term “infinite-dimensional” is used (Diehl and Gros, 2017). Despite the continuous time formulation of optimal control problems, numerical techniques to solve these problems typically require Newton-based iterative methods with finitely many decision variables and constraints. This can

be done by transcribing the infinite-dimensional problem into a finite-dimensional approximation (Betts, 2010).

Figure 2.1 illustrates the algorithm that is carried out when solving an optimal control problem using numerical techniques, and also provides context for the primary components required within the solver. This algorithm is known as the direct transcription process (Betts, 2010).

Figure 2.1: Optimal control problem direct transcription process. The error tolerance is typically a user-specified parameter.

2.2 Methods to Formulate Optimal Control Problems

There are numerous ways to formulate and solve optimal control problems. These can be broken down into trajectory optimisation and policy optimisation techniques. In trajectory optimisation, the control input is formulated as a function of time and an initial state is used to initialise the problem. On the other hand, in policy optimisation the control input is formulated as a function of state and the initial state is left arbitrary (Kelly, 2016a). Within these two methods, there are a variety of techniques that can be used to solve optimal control problems. Figure 2.2 showcases the main techniques (Betts, 1998; Diehl et al., 2005; Gardi et al., 2016; Kelly, 2015; Kelly, 2016a; Kelly, 2017; Tassa et al., 2014).

Figure 2.2: Optimal control problem solution techniques. Note that direct orthogonal collocation methods only fall under the umbrella of direct transcription.

2.3 General Formulation of a Trajectory Optimisation

Problem

Trajectory optimisation techniques are the focus within this thesis. A single-phase trajectory optimisation problem has been stated formally in Definition 2.1 (Betts, 1998).

Definition 2.1. Determine the state 풙(푡) ∈ℝ , the control 풖(푡) ∈ℝ , the initial time, 푡 ∈ ℝ, and final time, 푡 ∈ ℝ, that optimise the performance index

Bolza Form 퐽 = 휙 푡 , 푡 , 풙(푡 ), 풙푡 + 푔휏, 풙(휏), 풖(휏) 푑휏, Cost (2.1) subject to the following system of differential algebraic equations (DAEs)

System 풙̇ = 풇푡, 풙(푡), 풖(푡) Dynamics

Path 풄 ≤ 풄푡, 풙(푡), 풖(푡) ≤ 풄 Constraints (2.2)

Boundary 풃 ≤ 풃 푡, 푡, 풙(푡), 풙(푡) ≤ 풃 Conditions

The performance index, 퐽, has contributions from two terms. The integral term, 푔, is known as the Lagrange cost or the running cost, and the terminal cost term, 휙, is known as the Mayer cost. A combination of the two, as in the above, is known as a Bolza cost (Diehl and Gros, 2017). The decision variables are the initial and final time, as well as the state and control. The constraints are all formulated as inequalities; however, equality constraints can be enforced by setting the upper and lower bounds equal.

The above definition has been be expanded to include multiple-phases in Section 4.1. As illustrated in Fig. 2.2, numerous numerical techniques can be implemented to solve a problem that takes the form of Definition 2.1. These numerical techniques are discussed in further detail in Section 2.4.

2.4 Survey of Numerical Methods to Solve Trajectory Optimisation Problems

2.4.1 Dynamic Programming

Dynamic Programming (DP) is a mathematical optimisation technique developed by Richard Bellman and is based on the Principle of Optimality, which states that each sub-trajectory of an optimal trajectory is also optimal (Bertsekas, 2005). DP solves problems in the state space by making use of the Hamilton-Jacobi-Bellman (HJB) equation (Diehl and Gros, 2017). The idea behind the approach is to construct a grid over some time interval, and then solve the HJB equation recursively over a shortened horizon, starting from the end of the time interval (Kirk, 2012). The solution provides the optimal control policy, 풖∗, for the optimal trajectory, 풙∗, such that the cost-to-go function is optimised (Kirk, 2012).

The main advantage of DP is that globally optimal solutions can be obtained without the need to make assumptions about the differentiability or convexity of the dynamics and cost functions that define the problem (Diehl and Gros, 2017). A significant disadvantage of the DP algorithm is that computational costs can grow exponentially because the solution occurs in a high dimensional space. This issue was defined by Bellman as the “curse of dimensionality”, and is a severe issue for complex problems (Kirk, 2012).

2.4.2 Differential Dynamic Programming

Differential Dynamic Programming (DDP), like DP, is also described by the HJB equation. The technique was developed by Jacobson and Mayne (1970) and uses a quadratic extension of the classical DP approach to solve optimal control problems. This extension has allowed DDP to overcome the so-called “curse of dimensionality” that classical DP suffers from (Betts, 1998; Ozaki et al., 2015).

DDP is a second order shooting method that provides the optimal control only for the unconstrained control-space (Tassa et al., 2014). The method achieves locally-optimal solutions by repeatedly propagating the optimal control backwards in time along candidate trajectories to calculate an optimal cost-to-go matrix, and then forwards in time to compute the optimal solution. Each backwards pass satisfies optimality conditions and each forward pass satisfies the system dynamics (Mayne, 1966). This approach differs from transcription methods in that it combines the discretisation and optimisation steps as one (Kelly, 2016a).

Advantages of DDP are its robustness to poor initial guesses (Ozaki et al., 2015), as well as greater computational efficiency and scalability to higher dimensions compared to other global approaches, such as DP (Pan and Theodorou, 2014). Disadvantages of the DDP algorithm are its lack of ability to explicitly handle state and control constraints. This limits DDP’s practical functionality when compared with other transcription methods, such as collocation methods (Xie et al., 2017). This is an area of continued research.

2.4.3 Indirect and Direct Transcription Methods

Indirect transcription methods use the calculus of variations to first analytically construct the necessary and sufficient first-order optimality conditions, and then transcribe problems into discrete Hamiltonian boundary value problems (HBVPs) (Gardi et al., 2016). These first-order optimality conditions are derived using a Hamiltonian function, ℋ, which can be defined as

ℋ(퐱, 훌, 흁, 퐮, 푡) = 푳 + 훌퐟 − 흁퐂, where 푳 is a Lagrange cost term, 퐟 is the system dynamics, 퐂 is the vector of constraints, 퐱 is the state variable, 퐮 is the control variable, 훌 are the adjoined (or costate) variables, and 흁 are Lagrange multiplies associated with the constraints (Rao, 2009). The costates can be interpreted as Lagrange multiplies associated with the state equations. The Hamiltonian is used to replace the Bolza form cost of general optimal control problems with a function that relates the running cost to the dynamics and constraints (Hanson, 2007). The complete first-order optimality conditions using the Hamiltonian are derived in Chapter 4 of Betts (2010). The most well- known result within these conditions is Pontryagin’s Maximum Principle (Pontryagin, 1962).

In contrast to indirect methods, direct transcription methods first transcribe optimal control problems into finite-dimensional NLPs, and then construct the first-order optimality conditions. For an NLP, these optimality conditions are known as the Karush-Kuhn-Tucker (KKT) conditions* (Betts, 2010). This is summarised by saying that indirect methods optimise and then discretise, whereas direct methods discretise and then optimise (Betts, 2010; Kelly, 2017).

Indirect and direct methods both work based on basic calculus principles. For an objective function 휙(풙), indirect methods attempt to compute where 휙′(풙)=0. This differs from direct methods which only require a comparison between a sequence of points such that 휙(푥) > ∗ ∗ 휙(푥) >⋯> 휙(푥 ), where 푥 gives the optimum (Betts, 2010).

* The KKT conditions are provided in Section 2.5.1. 11

There are several difficulties in implementing indirect methods. Firstly, the convergence region for indirect methods is small as these methods are highly sensitive to initial guesses (Rao, 2009). Secondly, the user must supply guesses for the costate variables, but these are not physical quantities and as such are nonintuitive to estimate. Furthermore, indirect methods require the user to analytically compute state and control variable partial derivatives of the Hamiltonian, which can prove to be a challenging task for even experienced users (Betts, 2010).

The benefits of indirect methods over direct methods are that they lead to more accurate solutions and costate estimates when initialised sufficiently well (Gardi, 2015; Kelly, 2017). However, the use of direct methods is more prominent, as solving an NLP is typically easier than solving a HBVP, and techniques have been developed to improve the accuracy of solutions and costate estimates obtained through direct methods (Huntington et al., 2005). The direct transcription approaches illustrated in Fig. 2.2 are further detailed within Sections 2.4.4 – 2.4.7.

2.4.4 Direct Single Shooting

Direct single shooting (also known as direct shooting) is the simplest method for the transcription of an optimal control problem into an NLP problem. The method represents the entire trajectory as a single simulation with a set of dynamics, path constraints and a boundary constraint. The solution is propagated from an initial to final time using a guess, giving the method its name of “shooting”. The error in the boundary condition is evaluated and an NLP solver is used to choose the optimal trajectory (Betts, 1998). This method is effective for problems where the controls are smooth and there are only a small number of path constraints, such as orbital launch problems (Betts, 1998; Kelly, 2015).

2.4.5 Direct Multiple Shooting

One downfall of single shooting is that the method fails when applied to complicated problems where the states and controls cannot be approximated using single low order polynomials (Kelly, 2015). This gives rise to the simple extension to direct single shooting known as direct multiple shooting. The fundamental concept is to segment the trajectory into smaller regions and execute single shooting in each region. Equality constraints, known as defects, are then used to force continuity between the state trajectories at the end of one region and the start of the next (Betts, 1998; Betts, 2010). These defects lead to a vector root-finding problem where inputs are optimised such that the defects equal zero. This method results in enhanced robustness and can handle problems of higher complexity than direct single shooting.

Figure 2.3 illustrates the differences between single and multiple shooting. The defects required to be driven to zero are also shown.

Figure 2.3: Single shooting vs Multiple shooting (Figure adapted from Fig. 1 in Kelly, 2015).

2.4.6 Direct Collocation and Local Orthogonal Collocation

Direct collocation methods approximate the states and controls parametrically using polynomial splines and then satisfy the system dynamics and path constraints only at certain points, known as collocation points (Betts, 1998; Kelly, 2015). The dynamics are transcribed to be algebraic equality constraints just as in shooting methods. Common methods include implicit Runge-Kutta methods and orthogonal collocation methods.

In an orthogonal method, the collocation points are chosen as the roots of an orthogonal polynomial, such as a Chebyshev or Legendre polynomial (Rao, 2009). One approach of implementing this form of collocation is by segmenting the independent variable (typically time) and using a fixed-degree polynomial within each segment to approximate the states and controls. This is known as an h-method, or Local Collocation (Patterson and Rao, 2012b). Betts and Huffman (1998) provide examples of using h-methods on trajectory optimisation problems.

To demonstrate the above definition of direct collocation, consider the following initial value problem where the solution is required over the time interval [푡, 푡]

퐱̇(푡) = 퐟(퐱(푡), 푡), 퐱(푡) = 퐱.

Using direct collocation, the state is approximated by an 푁th-degree piecewise polynomial of the form

퐗(푡) ≈ 풙(푡) = a(푡 − 푡) , 푡 ∈ [푡, 푡],

where a are real-valued coefficients. By substituting this approximation into the differential equation, the approximated state derivative can be set equal to the right-hand side of the dynamics at the collocation points, (휏 …, 휏). The dynamics can be expressed as defects, 흃,

흃 = 퐗̇ 휏 − 퐟퐱휏, 휏 = ퟎ, 푗 = (1,…, 푁).

These defects are driven to zero whilst choosing inputs to optimise the objective (Rao, 2009).

2.4.7 Pseudospectral Collocation (Global Orthogonal Collocation)

In recent years, collocation methods that approximate trajectories with a single high-order polynomial have come to prominence. Their implementation is known as p-methods, or Global Collocation (Huntington et al., 2007b). Rather than increasing the number of segments as in an h-method, convergence in p-methods is realised by increasing the order of the polynomial with the number of segments fixed (Elnagar et al., 1995; Benson et al., 2006). Figure 2.4 shows a comparison between h- and p-methods.

Figure 2.4: Comparison between h- and p-Methods.

One commonly implemented p-method is Pseudospectral Collocation. In this approach, the state and control are approximated using Lagrange polynomials as global basis functions and the collocation points are chosen to be the zeros of an orthogonal polynomial (Kelly, 2017; Patterson and Rao, 2012b). For problems with smooth solutions, this method has the advantage that convergence occurs at an exponential rate, i.e. spectral convergence (Darby et al., 2010).

The most well-developed and implemented pseudospectral methods are those that collocate at the Legendre-Gauss (LG) points, the Legendre-Gauss-Radau (LGR) points or the Legendre- Gauss-Lobatto (LGL) points (Patterson and Rao, 2012b). The LG points are the distinct roots

of the 푛th-degree Legendre polynomial defined by 훹 (휏) = (휏 −1). The LGR ! points are the distinct roots of 훹(휏) + 훹(휏), whereas the LGL points are the distinct roots of 훹 (휏) combined with 휏 = ±1 (Rao, 2009). These are also known as the Associated Legendre Polynomials of 0th-degree and are all defined on the domain 휏 ∈ [−1,1] (Garg et al., 2017). Differences between the locations of the LG, LGR and LGL points are illustrated in Fig. 2.5.

Figure 2.5: Location of LG, LGR and LGL points (Garg et al., 2017).

Collocation methods that use the LG, LGR or LGL points are called the Gauss Pseudospectral Method (GPM) (Benson, 2004), Radau Pseudospectral Method (RPM) (Kameswaran and Biegler, 2008) and Lobatto Pseudospectral Method (LPM) (Ross and Fahroo, 2003), respectively. With 푛 collocation points, these methods result in a numerical integration (quadrature) scheme that can be used to exactly compute the integral of:

 A (2푛 − 1)th-order polynomial using the GPM (Benson, 2004),  A (2푛 − 2)th-order polynomial using the RPM (Quarteroni et al., 2000), and  A (2푛 − 3)th-order polynomial using the LPM (Quarteroni et al., 2000).

Garg et al. (2017) carried out comparisons of all three methods on two numerical examples, one with an analytical solution and one that has been studied extensively in literature. For the first problem, it was found that the GPM and RPM approximated the state to an accuracy of four orders of magnitude better than the LPM on average (error magnitudes were approximately

10 compared with 10). The GPM and RPM also approximated the control to an accuracy of two to seven orders of magnitude better than LPM (average error magnitudes were approximately 10 compared with 10). Additionally, the estimates of the costates, which are the Lagrange multipliers associated with the state dynamics, did not converge using the LPM. For the second example problem, the state and control in all three methods yielded similar accuracy. However, the LPM’s costate estimates were again seen to oscillate. Furthermore, Fahroo and Ross (2008) showed that the RPM is the only method of the three that is able to solve infinite-horizon optimal control problems. It achieves this through the use of an affine transform that maps the semi-infinite domain 푡 ∈ [0,∞) to the closed domain 휏 ∈ [−1,1].

Huntington et al. (2007a) also conducted an analysis and found a unique quality of the GPM. It was found that the GPM yields an equivalence between a NLP’s KKT conditions and a HBVP’s first-order optimality conditions which result from an indirect approach (previously outlined in Section 2.4.3). This means that the GPM yields costate estimates as accurately as indirect methods are known to do. Figure 2.6 illustrates the equivalence of these two conditions in a flow diagram. On the other hand, while the RPM has not been found to develop an exact equivalence between the KKT conditions and the HBVP’s first order optimality conditions, it does develop a near equivalence (Rao, 2009).

Figure 2.6: Flow diagram for the GPM’s equivalent optimality conditions for Direct and Indirect approaches (Adapted from Fig. 1 in Huntington et al., 2007a).

2.5 Nonlinear Programming

2.5.1 Formal Definitions

A review into most optimal control problems would not be complete without discussion about NLPs. Solving NLPs is one of the key elements in solving an optimal control problem. In fact, the advancements of optimal control solution techniques and NLP techniques have paralleled each other closely (Betts, 1998). The general NLP is stated formally in Definition 2.2.

Definition 2.2. Let 푛, 푚, 푝 ∈ℤ . Let 푿 ⊂ℝ , let 푭 ∶ ℝ →ℝ, 푔 ∶ℝ →ℝ and ℎ ∶ℝ →ℝ be nonlinear real-valued functions on 푿 for each 푖 ∈ {1,…, 푚 } and 푗 ∈ {1,…, 푝}. The NLP problem is to determine the 푛-dimensional vector of decision variables 퐱 = (푥,…, 푥) ∈ 푿 to minimise (or maximise) the scalar objective function,

푭(퐱), (2.3a) subject to the 푚 algebraic inequality constraints and the 푝 algebraic equality constraints

푔(퐱) ≤ 0, (2.3b)

ℎ(퐱) = 0. (2.3c)

The KKT first-order necessary conditions that must be satisfied for a solution vector 퐱∗ to be optimal are given in the following definition (Boyd and Vandenberghe, 2004).

Definition 2.3. For the NLP in Definition 2.2, suppose 푭 ∶ ℝ →ℝ, 푔 ∶ℝ →ℝ and ℎ ∶ℝ →ℝ ∗ are continuously differentiable at a local optimum 퐱 , then there exists constants 휇 and 휆 called the KKT multipliers for each 푖 ∈ {1,…, 푚 } and 푗 ∈ {1,…, 푝} such that

∗ ∗ ∗ ∇푭(퐱 ) + 휇 ∇푔(퐱 ) + 휆훻ℎ(퐱 ) = 0, (2.4a)

∗ Primal 푔(퐱 ) ≤ 0, (2.4b) Feasibility

∗ Primal ℎ(퐱 ) = 0, (2.4c) Feasibility

Dual 휇 ≥ 0, (2.4d) Feasibility

∗ Complementary 휇푔(퐱 ) = 0. (2.4e) Slackness

Eq. 2.4a describes the cancellation of the gradients between the objective and the constraints that are designated as being active at 퐱∗. Because only the active set* of constraints are included in this equation, those that are inactive have zero-valued KKT multipliers associated with them. This is implicitly stated by Eqs. 2.4b – 2.4e.

2.5.2 Algorithms to Solve NLPs

The two most common algorithms to solve NLPs are sequential quadratic programming (SQP) and interior point (IP) methods (Betts, 2010). These two methods are briefly summarised below to provide appropriate mathematical background in regard to the choice of NLP solver for the trajectory planning software.

2.5.2.1 Sequential Quadratic Programming

SQP is a gradient-based technique that uses active set schemes and applies Newton’s method to constrained optimisation problems in an analogous way to how Newton’s method is applied to unconstrained problems (Biggs, 1975; Han, 1977; Powell, 1978). To solve the NLP of Eq. 2.3, SQP uses an iterative approach where the NLP is approximated by a quadratic program

(QP) subproblem at approximate solutions, 퐱. The QP solution is then used to construct a ∗ sequence of better approximations, 퐱, that hopefully converge to the optimal solution, 퐱 .

The QP is generated at each iterate 퐱 using the Hessian of the Lagrangian function as a basis (Gill et al., 1981). Using the same variable definitions as Eq. 2.4, the Lagrangian is defined as

푚 푝 퐿(퐱, 휆, 휇) = 푭(퐱) + ∑푖=1 휇푖 푔푖(퐱) + ∑푗=1 λ푗 ℎ푗(퐱). (2.5)

At the KKT points, Eq. 2.5 reduces to the NLP’s objective function, 푭(퐱), which results in the critical points of the Lagrangian also being critical points of 푭(퐱). Hence, the search direction, 풅 ∈ℝ, in an SQP algorithm can be determined by minimising the following QP subproblem

min 훻퐹(퐱) 풅 + 풅 ∇퐿(퐱, λ, μ) 풅, (2.6a) 풅

s. t. 품(퐱) + ∇품(퐱) 풅 ≥ 0, (2.6b)

풉(퐱) + ∇풉(퐱) 풅 = 0. (2.6c)

* The active set consists of all constraints that will actually influence the final optimisation. An inequality constraint 푔(퐱) ≥0 is called inactive at x if 푔(x)>0. Equality constraints are always active. 18

This subproblem is much easier to approach than the parent NLP defined in Eq. 2.3 because it is a problem of only one variable that can be solved using any QP algorithm (Seefelder, 2002). The solution from Eq. 2.6 then provides the next iterate of the decision variable vector as

x = x + α푑. (2.7)

The parameter α is determined by a line search algorithm such that a sufficient decrease in some merit function is achieved (Boggs and Tolle, 1995).

Common algorithms using SQP methods include MATLAB’s fmincon and the commercially available code Sparse Nonlinear OPTimizer’ (SNOPT), which is developed by Gill et al. (2005). The reader is referred to Biegler (2010), Boggs and Tolle (1995) and Gill et al. (1981) for more comprehensive descriptions of the SQP method.

2.5.2.2 Interior Point Methods

IP methods are a class of algorithms that aim to solve constrained optimisation problems by solving a sequence of approximate problems (Shevade, 2012). These methods are also commonly referred to as Barrier methods. Unlike SQP, which considers active set strategies, IP methods deal with relaxed KKT conditions, known as primal-dual equations (Biegler, 2010). To provide a summary of an IP method, consider the following simple NLP

min 풇(퐱), (2.8a) 퐱

s. t. 풄(퐱) = ퟎ, (2.8b)

퐱 ≥ ퟎ. (2.8c)

Note the inequality constraints that were previously defined in Eq. 2.3 have here been cast into equality constraints through the use of non-negative valued slack variables; i.e., for 푔(퐱) ≤0, introduce slack variables 푠 ≥0 such that equality constraints can be formed as 푔(퐱) − 푠 =0 (Wächter and Biegler, 2006). The IP algorithm removes the inequality bounds on the decision variable by replacing Eq. 2.8 with a series of barrier problems of the form

(2.9a) min 퐵(퐱, 휇) = 풇(퐱) − 휇 ln(x) ; 휇 > 0

s. t. 풄(퐱) = ퟎ. (2.9b)

The logarithmic term acts as a penalty (barrier) function as it causes 퐵(퐱, 휇) to approach infinity if any of the x approach zero. Hence, this confines the algorithm’s search space to the interior of the region defined by Eq. 2.8c. The algorithm then approximates solutions for Eq. 2.9, where the so-called barrier parameter 휇 is driven towards zero. As 휇 →0, 퐵(퐱, 휇) converges towards solutions of Eq. 2.8. This approach is equivalent to applying a homotopy method* to the perturbed primal-dual system of Eq. 2.8 (Wächter and Biegler, 2006), which is given by

∇풇(퐱) + ∇풄(퐱)흀 − 풛 = 0, (2.10a)

풄(퐱) = ퟎ, (2.10b)

푿풁풆 − 휇풆 = ퟎ. (2.10c)

Here, 흀 and 풛 correspond to Lagrange multipliers for the constraints in Eq. 2.8b and 2.8c, respectively. 푿 and 풁 are diagonal matrices containing the elements of 퐱 and 풛 respectively and 풆 = (1,…,1). Solutions to Eq. 2.9 are typically approximated using Newton’s Method for a fixed value of 휇. The method then decreases 휇 and solves the next barrier problem using the previous solution (Wächter and Biegler, 2006). Note that when 휇 =0, Eq. 2.10 simplifies to the KKT conditions for Eq. 2.8.

Common implementations of IP methods include the open-source solver ‘Interior Point OPTimizer’ (IPOPT) which is developed by Wächter and Biegler (2006), as well as commercial codes such as KNITRO (Waltz et al., 2006) and LOQO (Vanderbei, 1999). For more detailed descriptions on IP methods, the reader is referred to Betts (2010), Biegler (2010), Schmidt (2015) and Wächter and Biegler (2006).

2.6 Derivative Calculation Methods

Derivative calculations are ubiquitous in the solution of NLPs as the evaluation of the objective function’s gradient, the constraint Jacobian and, in some cases, the Lagrangian Hessian are required in order to solve a problem. Three derivative calculation methods are used in literature:

 Finite-Differencing,  Complex-Step Differentiation, and  Automatic Differentiation (AD).

* The basic idea in homotopy methods is to solve a problem and use its solution as an initial guess for a similar problem. This assists in convergence as it is analogous to warm starting an optimisation. 20

2.6.1 Finite-Differencing

Finite-differencing can be said to be the most basic way to estimate derivatives for an NLP (Rao, 2009). One simple method is that of forward finite-differencing. Consider a vector function 풇 ∶ℝ →ℝ. The derivative of 풇, 휕풇/휕퐱, can be approximated with a variable stepsize forward finite-difference as

휕풇 풇(퐱 + 퐡) − 풇(퐱) ≈ , (2.11) 휕푥 ℎ(1 + |푥|)

th where the perturbation vector 퐡 arises by perturbing the 푖 component of 퐱 and is computed as

퐡 = ℎ(1 + |푥|)퐞. (2.12)

th The vector 퐞 is the 푖 row of the 푛 × 푛 identity matrix and the scalar ℎ is a chosen step-size (Patterson and Rao, 2012b; Gill et al., 1981). Second derivatives can be approximated in an analogous manner, except with perturbations in two variables, as follows

휕 풇 풇퐱 + 퐡 + 퐡 − 풇(퐱 + 퐡) − 풇퐱 + 퐡 ≈ . (2.13) 휕푥휕푥 ℎ (1 + |푥|)1 + 푥

While finite-differencing is suitable for some problems, it is an inefficient and often inaccurate approach. This can harm an NLP’s ability to converge to a solution (Rao, 2009).

2.6.2 Complex-Step Differentiation

Complex-step differentiation is a highly accurate approach to approximate derivatives. The approach was developed by Martins et al. (2003) and computes the first derivative of a function, 푓, by considering the imaginary components of a Taylor Series expansion of 푓 with a complex perturbation. The derivative is approximated as,

Im[ 푓(푥 + 푖ℎ) ] 푓(푥) ≈ . (2.14) ℎ

This gives an 풪(ℎ) estimate of the first derivative. Errors can be minimised by choosing the step-size ℎ to be extremely small, for example 1×10. Such small values of ℎ are possible as there are no subtractive cancellation errors, unlike finite-differencing approximations (Martins et al., 2003). 21

A limitation of the complex-step method is that double the memory is required to perform complex number arithmetic. This means that it is typically less computationally efficient than finite-differencing (Rao, 2009). A second limitation of the method is that it can only compute first derivatives.

2.6.3 Automatic Differentiation

The third approach considered is AD. This is an extremely powerful method that computes exact derivatives rather than approximations (Rao, 2009). Two common approaches to AD are source transformation and operator overloading (Bischof et al., 2002). In source transformation, the original code provided to the AD tool is pre-processed to a new code and derivatives are propagated through this new code using the chain rule (Bischof et al., 2002). Operator overloading is perhaps the more efficient means of applying AD, as programming languages with polymorphic features, such as MATLAB, allow in-built function semantics to be redefined in ways that facilitate the implementation of AD (Baydin et al., 2017). In this method, an overloaded class is defined within the language and functions are created that operate on objects within that class. Derivatives are then computed using the chain rule (Rao, 2009).

The term “automatic” has been a source of confusion as often any method or tool that does not involve manual differentiation is referred to as AD (Baydin et al., 2017). It must be stressed that AD is neither numerical differentiation or symbolic differentiation. Rather, it refers specifically to a group of techniques that compute derivatives by propagating the values through computer programs to generate numerical evaluations, as opposed to derivative expressions. That is to say, while AD does implement symbolic rules of differentiation, derivative values are tracked numerically, not as explicit expressions (Baydin et al., 2017). This leads to exact derivatives being computed rather than approximations. Further discussion on AD can be found in Baydin et al. (2017).

2.7 Grid Refinement

Referring back to Fig. 2.1, the steps involved in solving a trajectory optimisation problem are:

 Discretising the problem into a finite-dimensional NLP,  Solving the resulting NLP, and  Assessing the accuracy of the solution approximation and if necessary refining the discretisation.

The subject of this section is the final-step, known as grid refinement. The “grid” refers to the number of discretisation points used in the solution, as well as their distribution along the horizontal axis. Particular emphasis is placed on refinement within pseudospectral methods.

As with all numerical methods, errors are present in the solution of NLPs. For problems discretised using h-methods, the approach to improve the solution approximation is to increase the number of grid intervals along the trajectory. In p-methods, the approach is to increase the order of the approximating polynomial. This is illustrated in Fig. 2.7.

Zhao and Tsiotras (2011) described an h-method that uses splines of a fixed order, together with a probability density function, to generate a sequence of grids that are non-decreasing in size. Gong et al. (2008) proposed a p-method that uses a pseudospectral differentiation matrix* to identify discontinuities in the solution, and then refines the grid appropriately.

Figure 2.7: h- and p- methods of grid refinement. The h-method of refinement shows the increase of intervals to approximate the trajectory using splines. The p-method adds collocation points using a single polynomial in order to achieve desired fidelity.

While h-methods are effective at achieving convergence in problems with non-smooth solutions, and p-methods are effective for smooth problems, both methods have limitations. To achieve a desired error tolerance, an h-method may necessitate an extremely dense grid, whereas a p-method may require an extremely high order polynomial. Hence, both approaches can impose a significant computational burden and lead to a large NLP. Furthermore, for non- smooth problems, a p-method can result in poor approximations even with a high degree polynomial due to slowed convergence rates (Darby et al., 2010).

* Differentiation matrices are described in Section 4.2.2. 23

Advanced hybrid methods, known as adaptive hp- or ph-methods, have been developed to minimise the computational burden and the size of the NLP that can occur through grid refinement, whilst still exploiting the individual advantages of h- and p-methods (Patterson et al., 2014). In these hybrid methods, the partition of the grid and the polynomial order are both variable. Whether a method is referred to as hp- or ph- depends upon the order in which the refinement occurs. For example, an algorithm is labelled ph if it attempts to first increase the polynomial order and then refine the grid interval widths. The reverse holds for hp.

Darby et al. (2010) developed a hp- approach that first computes the discretisation error and then sweeps across each grid interval to identify locations where the error is significantly larger than other points. The grid is then subdivided at these identified locations. If the error is uniform across an interval (but still above a specified tolerance), then the polynomial order is increased.

Patterson et al. (2014) developed a simple yet effective ph- approach that computes the relative discretisation error and then attempts to increase the polynomial order in those intervals that do not satisfy some error tolerance. To prevent the polynomial order from becoming too large within a grid interval, an upper limit, 푁, is specified. If the required polynomial order to satisfy an error tolerance is above 푁, then the grid interval is subdivided. If it is below

푁, the old polynomial order is set to the new required amount.

In the hybrid methods investigated, the computation of the relative discretisation error within each grid interval was a key step. As the state variable is defined by a unique function approximation, given by the system dynamics equation, the error can be calculated as a function of the state (Patterson et al., 2014). The idea is to compare the state estimate from an 푛-th order approximation, say 푦(푡), with that of an (푛 + 1)-th order approximation, 푦(푡). The absolute and relative errors in interval 푘 are then given by Eq. 2.15a and Eq. 2.15b, respectively,

퐸 = |푦(푡) − 푦(푡)|, (2.15a)

퐸 푒 = . (2.15b) (1 + max|푦(푡)| )

Patterson et al. (2014) has shown that ph-methods yield better approximations than hp- methods. In fact, for a test problem where very high accuracy solutions were required, the hp- approach developed by Darby et al. (2010) led to computationally intractable and unreliable grid formulations, whereas the ph- approach converged successfully.

2.8 Techniques to Scale Optimal Control Problems

A major component of successfully solving NLPs is ensuring that the problem is well-scaled. Betts (2010) has shown that poorly scaled problems can lead to convergence rates growing significantly or solutions simply diverging. While there is no set procedure to scaling an optimal control problem, certain methods developed over time have been observed to work consistently well on a range of problems. Betts (2010) recommends scaling all of the optimal control variables by their upper and lower bounds in such a way that the scaled variables lie on the interval [−1/2, 1/2]. This is done in order to ensure that the variables and first-order derivatives are all ≈ 풪(1), as also recommended by Gill et al. (1981). An approach to scale the variables onto this domain is given in Eq. 2.16. Let 푥 ∈[푎, 푏], then

푥 − 푏 1 푥̅ = + ∈ [−1/2, 1/2]. (2.16) 푏 − 푎 2

The objective gradient, constraint Jacobian and Lagrangian Hessian can then also be scaled in analogous ways (Betts, 2010).

As most optimal control problems are formulated with governing equations that use Newton’s and Euler’s laws, canonical transformations of the units can also be implemented to scale a problem (Rao, 2009). These scaling procedures are typically preferable over general methods of scaling, but, as they are problem specific, it is not possible to implement a general scheme within the trajectory planning software.

2.9 Available Trajectory Optimisation Software

Numerous trajectory optimisation software packages are available for a variety of programming languages. Table 2.1 lists those that this author is aware of.

Table 2.1: Existing software packages. Software Language License Transcription Derivatives Solvers Reference

MATLAB, IPOPT, CasADi Python, LGPL MS, DC AD KNITRO, Andersson (2013) C++ SNOPT

Non- SNOPT, Stryk and Bulirsch DIRCOL Fortran DC FD Commercial NPSOL (1992)

Ross and Fahroo DIDO MATLAB Commercial P FD Built-in (2002)

fmincon, Cizniar et al. dynopt MATLAB OS P FD IPOPT (2005)

MATLAB, Gath and Well GESOP Commercial P Unknown SNOPT C, Fortran (2001)

AD, IPOPT, Patterson and Rao 픾ℙ핆ℙ핊 − 핀핀 MATLAB Commercial P FD, US. SNOPT (2012b)

ICLOCS MATLAB OS MS, DC FD fmincon Falugi et al. (2010)

Hoffmann et al. MUSCOD-II C, Fortran Commercial MS AD QPOPT (2011)

Inagawa and OpenGoddard Python OS P Unknown SLSQP Sakaki (2017)

OptimTraj MATLAB OS MS, DC, OC FD, US fmincon Kelly (2016b)

2 Clause opty Python DC FD IPOPT Moore (2018) BSD

US Export Hargraves and OTIS Fortran P Unknown SNOPT Controlled Paris (1987)

KNITRO, Rutquist and PROPT MATLAB Commercial P FD SNOPT Edvall (2010)

AD, FD, IPOPT, 퓟퓢퓞퓟퓣 C++ GPL P Becerra (2009) US SNOPT

핊핆ℂ핊 Fortran Commercial MS, DC FD, US Built-in Betts (2010)

MS = Multiple Shooting, DC = Direct Collocation, OC = Orthogonal Collocation, P = Pseudospectral, OS = Open-Source, FD = Finite-Difference, AD = Automatic Differentiation, US = User-supplied.

Each of the programs in Table 2.1 offer various attributes and features that warrant them useful for different problems. From the survey of available software, it has been identified that there is a lack of open-source software for MATLAB that implement pseudospectral methods and use open-source NLP solvers with performance benchmarks rivalling those of commercial solvers. One such open-source solver is IPOPT. Furthermore, out of all the open-source software available for MATLAB, only CasADi has the ability to solve multiple-phase trajectory optimisation problems. However, CasADi only implements low order transcription methods. 픾ℙ핆ℙ핊 − 핀핀, 풫풮풪풫풯 and 핊핆ℂ핊 are the only other software that can solve multiple-phase problems. The multiple-interval, multiple-phase solver developed within this thesis will attempt to fill these voids, and the open-source nature of the software will allow users the freedom to make modifications to fit their needs.

2.10 Chapter Summary

This chapter has presented a summary of relevant literature for the development of the trajectory planning software. The main areas of review were the discretisation of an optimal control problem into a finite-dimensional NLP, the optimisation algorithms available to solve an NLP, approaches to calculate derivatives and, lastly, grid refinement methods to improve the convergence rates and accuracy of solutions.

There is a large body of work regarding trajectory optimisation and as such the review presented in this thesis is not exhaustive. However, additional information and sources of information have been cited where possible. Strong focus has been placed upon pseudospectral methods, and a scope to develop a MATLAB program that implements these methods has been identified. In particular, several solutions have been presented for the choice of numerical method to transcribe the trajectory optimisation problem into an NLP. Subsequently, two optimisation algorithms to solve the resulting NLP have been outlined, accompanied by the identification of three techniques to calculate derivatives. Lastly, two grid refinement methods have been described along with a tried and testing technique to scale optimal control problems. This provides motivation and context for the goals and methodology regarding the development of the trajectory planning software, which is the subject of Chapters 3, 4 and 5.

Chapter 3 Required Components for Software and Design Decisions

In this chapter, design decisions regarding the required and desired components for the software are outlined. This forms the overarching plan of how the software will be developed.

3.1 Overview of Planned Components

The components required for the software have been decided upon via the knowledge gained through the literature review, which identified common features and voids left by current open- source software. The main considerations are:

 The design of the user interface to the solver,  The choice of numerical method to transcribe the problem into an NLP,  The choice of NLP solver,  The choice of derivative calculation methods,  The approach to automatically scale a problem, and  The choice of grid refinement algorithm.

The above also lists the goals that have been set regarding the components desired for the software. An additional goal is to ensure simplicity in the software’s user interface. The design decisions and methods to implement the components and achieve these goals are now outlined.

3.1.1 Influence on Software Structure from Trajectory Optimisation Problem

The means through which a trajectory optimisation problem is passed to the software dictates much of the software’s structure. From the general formulation of a trajectory optimisation problem, defined in Eq. 2.1 and Eq. 2.2, it can be seen that separate MATLAB functions must be specified for the system dynamics, path constraints, boundary constraints, and the cost functions. The boundary functions (boundary constraints and Mayer cost) only require the boundary state and time as arguments. The arguments for the other three functions are the state, control and time evaluated at the collocation points. 28

To ensure simplicity in the call to the software, a single structure array will be used to store all problem-related data because it allows for information to be transferred in an efficient and intuitive manner. Initially, cell arrays were considered to store phase-dependent data. However, the ability to call information by name using structure arrays allows for greater convenience.

3.1.2 Choice of Numerical Transcription Method

Figure 3.1 presents a decision tree which illustrates the choice of numerical transcription method. It also details the choices made regarding the various techniques that can be used to implement the chosen method.

Figure 3.1: Decision tree for choice of transcription method (choices are shown with a red outline). 29

From the literature review into the various numerical methods to solve trajectory optimisation problems, it was found that collocation methods are the most powerful and frequently used methods (Rao, 2009). In particular, as outlined in Fig. 3.1, pseudospectral methods implementing either the GPM, RPM or LPM are appealing due to their spectral convergence rates and highly accurate numerical integration schemes (Garg et al., 2017). Of these three methods, the GPM and RPM achieve more reliable and accurate solutions than the LPM (Garg et al., 2017).

The RPM is the most suitable discretisation approach for a multiple-interval, multiple-phase problem. This is because the RPM collocates one end point (the initial point), which allows for the entire span of a trajectory to be discretised. This is achieved by setting the first point in one interval, or phase, to also be the terminal point in the previous interval, or phase. Figure 3.2 illustrates this further. As a consequence of this discretisation, interpolations are not required to obtain solution approximations at the start and end of each grid interval (or phase), as would be required by the GPM (Li et al., 2017). In addition, this interpolation step would result in the following two issues occurring at the end points of each grid interval and phase for the GPM:

 Sub-optimal state and control approximations would be achieved, and  There is no guarantee that constraints would be satisfied (Huntington, 2007).

Figure 3.2: Example of a multi-interval, multi-phase problem using the RPM. There is one interval in first phase, two in the second and three in the third. This example shows how the RPM distributes points across different intervals and phases. The final point in each interval and phase is non-collocated; however, since the first point in the next interval or phase is collocated, an approximation is still obtained for the end point. This results in there being only one non-collocated point for any problem: 휏 = +1. 30

Within the software, the Radau collocation points and the Radau Differentiation matrix are to be calculated using a package known as “A MATLAB Differentiation Matrix Suite”, developed by Weideman and Reddy (2000). The roots of the Legendre polynomials are well- known to be the eigenvalues of a square tridiagonal Jacobi matrix. The package by Weideman and Reddy (2000) uses this to efficiently compute the Radau points.

3.1.3 Choice of NLP Solver

The two NLP optimisation algorithms investigated within the literature review were SQP and IP methods. An advantage of IP methods over SQP is that they do not succumb to the potential bottlenecking caused by identifying the active set of constraints, or the computational complexity of solving a QP for large problems (Albuquerque et al., 1999). However, the choice of using either of these two algorithms is problem dependent and one method can outperform the other in different situations. A comparison between IP and SQP solvers was carried out by Mittelmann (2018), and a summary of the results is presented in Table 3.1.

Table 3.1: Summary of a comparison of IP and SQP solvers by Mittelmann (2018). NLP Solvers Outcomes Compared

- IPOPT was seen to perform as well as or better than SNOPT on a range of test problems. This is because SNOPT does not use exact second derivatives, and IPOPT (IP) and also casts problems into heavily constrained and computationally expensive QPs SNOPT (SQP) (Bielger, 2010). - The reverse was true in some other cases with SNOPT being more efficient than IPOPT.

- fmincon was outperformed by IPOPT in nearly every example problem tested. IPOPT (IP) and In fact, for some examples, fmincon exceeded the prescribed CPU time limit of fmincon (SQP) 2 hours, whereas IPOPT was able to solve the problem in a matter of seconds.

For the trajectory planning software, IPOPT is the leading choice of NLP solver because it is an open-source C++ code with an accessible MATLAB interface, and possesses performance benchmarks better than, or equal to, numerous other solvers. This includes fmincon, which is presently used by many open-source MATLAB-based solvers, such as ICLOCS and OptimTraj.

The requirements posed by IPOPT in order to successfully call the algorithm and solve a problem have the largest impact on the structure of the trajectory planning software. IPOPT requires that the NLP be set up as

min 풇(퐱) (3.1a) 퐱

s. t. 퐱 ≤ 퐱 ≤ 퐱, (3.1b)

풄 ≤ 풄(퐱) ≤ 풄. (3.1c)

This format necessitates that problems be constructed using a single vector of decision variables, 퐱, a single objective function, 풇, a single constraint function, 풄, and four vectors that contain the upper and lower bounds on the variables and constraints. Therefore, the formulation of the parent trajectory optimisation problem requires that bounds be provided for each element of 퐱 and also for every constraint. These vectors must contain information for every phase of the trajectory optimisation problem. Note: despite the fact that the constraints are all posed as inequalities, IPOPT allows equality constraints to be enforced by setting 풄 = 풄.

Furthermore, as IPOPT is a full Newton solver, three functions must be provided to the solver that compute the objective gradient, constraint Jacobian and Lagrangian Hessian. Due to the fact that IPOPT stores the matrices in sparse formats, the sparsity structure of the Jacobian and Hessian must also be provided. The sparsity of a problem is contingent upon numerous factors, such as the variable dependency of the dynamics and constraints functions, the number of collocation points, the grid structure and the number of phases. These factors must all be accounted for when constructing the sparsity structures.

3.1.4 Choice of Derivative Calculation Methods

In Section 2.6, three techniques were outlined to carry out derivative calculations. While finite- differencing will be suitable for some problems, it is inefficient and produces 풪(ℎ) errors. This can cause convergence difficulties for an NLP solver (Biegler, 2010). Hence, all three approaches will be implemented in the trajectory planning software. This allows problems to be solved with different techniques if one fails to yield a satisfactory solution. The open-source package MatlabAutoDiff (de La Gorce, 2016) will be integrated into the solver to implement AD. Due to the fact that complex-step differentiation and MatlabAutoDiff cannot compute second derivatives, all Hessian matrix approximations will be carried out using forward- differencing. As Table 2.1 shows, the majority of current packages only implement at most two 32

derivative calculation methods. Hence, the use of three methods, and in particular complex-step differentiation, will be a feature that is unique to the software developed within this thesis.

3.1.5 Problem Scaling Approach

An in-built scaling procedure will be implemented in the software using the technique that is posed by Betts (2010) and outlined in Section 2.8 of this thesis. The user will be provided the following options to scale a problem: (i) use the in-built scheme, (ii) use their own scheme, or (iii) not scale a problem at all. Scaling is another feature not implemented in many other open- source packages, such as OptimTraj.

3.1.6 Choice of Grid Refinement Approach

In Section 2.7, two adaptive grid refinement techniques were presented for pseudospectral collocation methods. As outlined, the ph-method developed by Patterson et al. (2014) was the superior of the two and produces more accurate solutions. Hence, due to this, and due to its ease of implementation, a ph-method has been chosen for the software. The approach is similar to that of Patterson et al. (2014) but with modifications made to the relative error estimate where an equation described by Betts (2010) is used in place of Eq. 2.15.

The grid refinement algorithm is required to carry out the following tasks:

1. Calculate discretisation error across each grid interval in every phase, 2. Decide if the interval needs to be subdivided or the polynomial order increased, 3. Create a new grid structure for the next optimisation iteration based on Step 2.

The designed algorithm is outlined in Section 5.1 of Chapter 5. This algorithm is an innovative feature for the software as numerous commercial and open-source packages, such as PROPT, MUSCOD-II, OpenGoddard and OptimTraj do not contain an automatic grid refinement algorithm.

Chapter 4 Mathematical Development of Method

This chapter presents a formulation of a general multiple-phase trajectory optimisation problem as well as a formulation of the multiple-interval discretisation of the RPM. In Section 4.3, these formulations are then transcribed into the specific NLP that will ultimately be solved by IPOPT.

4.1 Formulation of General Multiple- Phase Trajectory Optimisation Problem

Expanding on the definition of a single-phase trajectory optimisation problem posed in Section 2.3, the general continuous time multiple-phase trajectory optimisation problem can be formulated as follows (Betts, 1998; Betts, 2010; Rao et al., 2010).

Let 푝 ∈ {1,…, 푃} be a single phase from a set of 푃 phases. The notation ( . )() is utilised to represent terms in the 푝th phase. The Bolza form cost to minimise, or maximise, is given by

( ) ( ) ( ) ( ) ( ) 퐽 = 휙() 푡 , 푡 , 풙() 푡 , 풙() 푡 + 푔() 푡, 풙()(푡), 풖()(푡) 푑푡 (4.1a) ()

This cost is subject to:

풙̇ ()(푡()) = 풇()푡(), 풙(), 풖() Dynamic Equality Constraints

() () () () () () 풄 ≤ 풄 푡 , 풙 , 풖 ≤ 풄 Path Constraints

() () () () 풃 ≤ 풃() 푡 , 푡 , 풙(), 풙() ≤ 풃 Boundary (4.1b) Constraints

() () () () () () 푡 , 푡 , 풙 푡 , 풙 푡 , … 휹() ≤ 휹() ≤ 휹() Phase Linkage () () () () Constraints 풖 푡 , 풖 푡

() () where 풙() = 풙()푡() ∈ℝ풙 , 풖() = 풖()푡() ∈ℝ풖 and 푡() ∈ℝ are the states,

() controls and time within phase 푝, respectively. Note that 푁풙 is the number of states in the 푝th () phase and 푁풖 is the number of controls in the 푝th phase. Also note that 휓 ∈ {1,…, 푃 −1}, 34

which restricts the phase linkages to be sequential. The functions have the following

( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) dimensions: 휙 ∈ℝ, 푔 ∈ℝ, 풇 ∈ℝ풙 , 풄 ∈ℝ풄 , 풃 ∈ℝ 풃 and 휹 ∈ℝ 휹 ,

() () where 푁풄 and 푁풃 are the number of path constraints and boundary conditions in phase 푝, () and 푁휹 is the number of linkage constraints across link 휓.

4.2 Formulation of Multiple-Interval, Multiple-Phase

Radau Pseudospectral Method

4.2.1 Multiple-Interval Formulation of Objective and Constraints

The multiple-phase problem defined in Eq. 4.1 can now be further partitioned into multiple- grid intervals and formulated using RPM discretisation.

It is known that the LGR collocation points exist on the half-open interval [−1,1). As such, the first step in deriving the multiple-phase RPM formulation is to map the time interval in each

() () () () phase from 푡 ∈ 푡 , 푡 to 휏 ∈ [−1,1], with the point at 1 being non-collocated. This is achieved using an affine transformation given by

() () 2 푡 + 푡 휏() = 푡() − . (4.2) () () 2 푡 − 푡

Now, suppose that the transformed domain 휏() is partitioned into 퐾() number of grid

() () () () intervals. Grid partitions 풮 = 푇, 푇 , 푘 = 1,…, 퐾 , are defined with the properties

푆() ∪ 푆() ∪…∪ 푆() = [−1,1] and 푆() ∩ 푆() ∩…∩ 푆() =∅, () () such that −1 = 푇() < 푇() <⋯< 푇() =1 are the grid interval partition points within a () phase, and the terminal point of each phase 푇() =1 is the non-collocated point. Note that the () notation ∪ represents the union, whereas ∩ represents the intersection.

() () For the definition of the multiple-interval formation, 풙 and 풖 will be used to represent the state and control vectors in the 푘-th grid interval of the 푝-th phase, respectively.

() () () () () Furthermore, the state at the boundary values in the 푡 ∈ [푡 , 푡 ] domain, i.e. 풙 (푡 ) and 풙()(푡()), are now expressed as 풙()(−1) and 풙() (+1), respectively. ()

By performing Radau collocation on each grid interval 푘, where 푘 = 1,…, 퐾() with 퐾() representing the final grid interval in phase 푝, the general continuous time multiple-interval, multiple-phase trajectory optimisation problem can be expressed as

퐽 = 휙() 푡(), 푡(), 풙()(−1), 풙() (+1) ()

() ( ) () () (4.3a) 푡 − 푡 + 푔()휏, 풙(), 풖() 푑휏 ( ) 2 with the cost of Eq. 4.3a subject to

() () 푡 − 푡 Dynamic Equality 풙̇ () = 풇() 휏(,), 풙(), 풖(), 2 Constraints

() () (,) () () () 풄 ≤ 풄 휏 , 풙 , 풖 ≤ 풄 , Path Constraints

(4.3b) () () () () () () () Boundary 풃 ≤ 풃 푡 , 푡 , 풙 (−1), 풙 ()(1) ≤ 풃 , Constraints

() () () () 푡 , 푡 , 풙 (−1), 풙 ()(1), … () () () Phase Linkage 휹 ≤ 휹 ≤ 휹 , 풖()(−1), 풖() (1) Constraints ()

() () () for 푘 = 1,…, 퐾 , 푝 ∈ {1,…, 푃} and 휓 ∈ {1,…, 푃 −1}. Note that the (푡 − 푡 )/2 factor on the Lagrange cost and dynamic constraints is independent of the grid intervals. Due to the fact that the state is required to be continuous across grid intervals, the state at the partition

() () () () point of the 푘-th and (푘 +1)-th grid intervals must be equal, i.e. 풙 푇 = 풙 푇 ,

() () where this holds for 푘 = 1,…, 퐾 −1. Recall that 푇 is the partition point at the interface of interval 푘 and 푘 +1. This constraint can be formulated implicitly by using the same variable

() () () () for 풙 푇 and 풙 푇 . This is demonstrated further in Fig. 4.1.

() The 푛 number of LGR collocation points within the 푘-th grid interval of the 푝-th phase are defined as the set

() (,) (,) 풞 = 휏 ,…, 휏 () ∈ [−1,1).

By the definition of pseudospectral collocation, the constraints of 4.3b are only required to be satisfied at these collocation points. However, the system dynamics of Eq. 4.3b must still be evaluated at the terminal time, which occurs at +1. Hence, the discretisation points of the NLP are defined as the superset of the collocation points and the terminal time. That is, the

() discretisation points, 풟 , are

() () (,) 풟 = 풞 , 휏 () , where 휏(,) = 푇() = 휏(,) =1. () ()

Therefore, even though the Radau collocation points do not include the terminal time, due to the fact that the discretisation points do, the state at the terminal time is still approximated within the RPM and thus optimised within the NLP.

To illustrate the multiple-interval formulation further, consider Fig. 4.1. In this example, two grid intervals have been used to approximate a state trajectory within a single-phase problem (meaning that 퐾() =2 with 푝 =1). In each grid interval, 푁 LGR collocation points have been used (resulting in 푁 +1 discretisation points in each interval). Note that different numbers of LGR points can be used in across intervals, but the same number has been used here for simplicity. For added clarity, the subscripts on the time variable, 휏, have been shown in green. These represent the discretisation points. Superscripts and subscripts in red represent the grid interval number, whereas superscripts in black represent the phase number (which is

() () () simply 1). The three grid partition points, 푇 , 푇 , and 푇 , have been shown with large circles. The first two of these partition points are also collocation points, because the RPM collocates the initial point in each interval. Hence, the partition point at the interface of the two

() intervals, 푇 , acts as the last discretistaion point for interval 1 and as the first discretistaion point for interval 2. Recall that this is one of the main advantages of the RPM as it allows for continuity between intervals (see Section 3.1.2 for further details). The remaining collocation points have been shown with black squares. Lastly, the non-collocated point at the terminal time is indicated with a blue circle. 37

Figure 4.1: Example of a two-interval formulation for the state trajectory. The state across grids is constrained to be equal and the same variable is used to formulate this constraint. 푁 LGR collocation points, resulting in 푁 +1 discretistaion points, have been used in each interval. The initial point of the

(,) (,) first interval, 휏 , corresponds to −1, whereas the final point of the second (i.e. final) interval, 휏 , corresponds to +1. The large circles represent partition points. Black squares represent collocation points. The blue circle represents the non-collocated point. The first two partition points are also collocation points. Subscripts in green are indexes for the discretisation points. Superscripts and subscripts in red represent the two grid intervals. The black superscripts indicate the phase number. Note that whilst a state trajectory is shown here, the control trajectory is formulated the exact same way. The only difference is that the final non-collocated point is not included, as RPM does not obtain a control estimate at this point. If this problem was extended to include 푃 number of phases, the only non- collocated point would be the terminal point of the 푃th phase.

4.2.2 State Approximation using the RPM

() Within the RPM, the state in grid interval 푘 and phase 푝 is approximated using an (푛 +1)-

* (,) th order basis of Lagrange interpolating polynomials , ℓ풙 . This approximation is given by,

() () () () (,) (4.4) 풙 (휏) ≈ 퐗 (휏) = 퐗 (휏) ℓ풙 (휏),

() () where 퐗 (휏) is the state evaluated at the discretisation points {풟 }, and the Lagrange interpolating polynomial basis is

( ) (,) 휏 − 휏 ( ) (,) ( ) ℓ풙 휏 = (,) (,) ∈ℝ . (4.5) , 휏 − 휏

(,) () Note that the above includes the non-collocated point 휏 () =1. Differentiating 퐗 (휏) with respect to 휏 gives the state derivative, i.e.,

() () ( ) 푑 () () 푑 (,) (,) () (,) 퐗 (휏) = 퐗 (휏 ) ℓ (휏) = 푫 퐗 (휏 ) ; 푫 ∈ ℝ (4.6) 푑휏 푑휏 풙

( ) ( ) ( ) × ( ) Here 푫 , ∈ℝ is known as the LGR Differentiation matrix, which computes the

() derivative of the Lagrange interpolating polynomials at the set of LGR collocation points, 풞 . Evaluating the derivative for the 푖th Lagrange interpolating polynomial at the 푚th LGR point,

() where 푚 ∈ {1,…, 푛 } , gives

() ( ) ∏ 휏(,) − 휏(,) (,) 푑 (,) (,) ,, 푫, = ℓ풙 휏 = () ∈ ℝ. (4.7) 푑휏 (,) (,) ∏, 휏 − 휏

Hence, the state derivative vector at the 푚th collocation point can be expressed as

() 푑 퐗() 휏(,) = 푫(,) 퐗() 휏(,). (4.8) 푑휏 ,

* Lagrange interpolating polynomials are tools to construct polynomials that pass through a desired set of points. 39

4.2.3 Control Approximation using the RPM

() The vectorised control in grid interval 푘 and phase 푝 is approximated by an (푛 )-th order basis (,) of Lagrange interpolating polynomials, ℓ풖 , and is given by

() () () () (,) (4.9) 풖 (휏) ≈ 퐔 (휏) = 퐔 (휏) ℓ풖 (휏),

() () where 퐔 (휏) is the control evaluated at the collocation points 풞 , and the Lagrange interpolating polynomial basis is

( ) (,) 휏 − 휏 ( ) (,) ( ) ℓ풖 휏 = (,) (,) ∈ℝ . (4.10) , 휏 − 휏

() The control is discretised at the 푛 collocation points of each grid interval in each phase. () The value of the control at the terminal boundary of a trajectory, 풖 ()(+1) , can be approximated via polynomial extrapolation after the NLP optimisation step is complete (Huntington, 2007).

4.2.4 Total Nonlinear Problem Discretisation using the RPM

The total NLP discretisation is accomplished by using the state and control approximations to transform the continuous time optimal control problem outlined in Eq. 4.3 into a discrete NLP.

Collocating the dynamic state equality constraints of Eq. 4.3b using the discretisation derived in Section 4.2.2, the differential dynamics equations can be expressed as the following algebraic defect constraints

( ) (푝) (푝) 푡 − 푡0 푫(,) 퐗() 휏(,) − 푓 풇() 휏(,), 퐗(,), 퐔(,) = ퟎ, (4.11) , 2

() where 푚 ∈ 1,…, 푛 . The constraints of Eq. 4.11 are required to be satisfied at each of the

() 푛 LGR collocation points.

With this formulation, the continuous time Bolza form cost of Eq. 4.3a can be approximated in discrete form as

( ) ( ) ( ) ( ) 퐽 = 휙() 푡 푝 , 푡 푝 , 풙 푝 (−1), 풙 푝 (1) 0 푓 1 퐾(푝) (4.12) ( ) (푝) () () 퐾 푡 − 푡 + 퓌(,) 푔() 휏(,), 풙(,), 풖(,), 2 푘=1

(,) where the Radau-quadrature weights, 퓌 , are computed as

(,) 1 퓌 = 2 . (4.13) (,) ′ (,) 1 − 휏 훹 () 휏

The first derivative of the 푞th order Legendre-Polynomial, 훹, is defined recursively as

(1 − 휏 )훹 = 푞훹 − 휏훹. (4.14)

This concludes the RPM discretisation of the multiple-interval, multiple-phase trajectory optimisation problem posed in Eq. 4.3. To summarise, the total discretisation of a trajectory optimisation problem - which is passed to the software by a user in the form of Eq. 4.1 - using the RPM results in the requirement to optimise the cost of Eq. 4.12, subject to the state dynamic defect constraints of Eq. 4.11, as well as the inequality path, boundary and linkage constraints defined in Eq. 4.3b. Equality constraints can be enforced by setting the upper and lower bounds on the constraints to be equal.

4.3 Conversion of Radau Pseudospectral Method Formulation into NLP Algorithms

The RPM discretised form of the general multiple-interval, multiple-phase trajectory optimisation problem from Section 4.2 is now transcribed into the specific NLP required by IPOPT. The NLP equations were previously defined in Eq. 3.1, Section 3.1.3.

4.3.1 Construction of Decision Variable Vector and its Upper and Lower Bounds

The decision variables are defined as the set of NLP variables required to be optimised by IPOPT. For the RPM discretised multiple-interval, multiple-phase trajectory optimisation problem, the decision variables are defined as

퓩 = 픃(), … , 픃() . (4.15)

The structure of the NLP is the same regardless of the number of mesh intervals and collocation points used to solve a problem. Hence, the total number of collocation points within a phase,

() () () 푁 = ∑ 푛 , can be used in the following definitions. The decision variables in phase 푝 are given by

() () () () () () () () () 풙 풖 (4.16) 픃 = 픃풙 , 픃풖 , 푡 , 푡 ∈ ℝ .

() () where 푝 ∈ {1,…, 푃}. The column vector 픃풙 contains the 푁풙 number of states at the 푁 +1 () () discretisation points, and the column vector 픃풖 contains the 푁풖 number of controls at the 푁 collocation points. More formally, these vectors are defined as

() () () 픃풙 = 퓧 , … , 퓧 (), (4.17) 풙

() () () where each vector from the 푁풙 states, 퓧 , (q=1,…, 푁풙 ), has one term at each of the

() () 푁 +1 discretisation points. That is, the elements of each 퓧 are populated by evaluating the qth state variable, 푥, at each of the discretisation points as follows

() () () () () 퓧 = 푥 , … , 푥 ; 푥 = 푥 (휏 ), ~푖 = 1, … , 푁() + 1. (4.18) , ,() ,

Similarly, the control vector is defined as

() () () 픃풖 = 퓤 , … , 퓤 (), (4.19) 풖

() () with the elements of each 퓤 , q=1,…, 푁풖 , being populated by evaluating the 푞th control variable, 푢, at each of the collocation points as follows

() () () () () 퓤 = 푢 , … , 푢 ; 푢 = 푢 (휏 ), ~푖 = 1, … , 푁(). (4.20) , ,() ,

The upper and lower bound vectors on the decision variables, 퓩 and 퓩, are defined in the exact same way as 퓩. Within the context of the NLP, the decision variable vector and its bounds form Eq. 3.1b.

4.3.2 Construction of Constraint Vector and its Upper and Lower Bounds

As per Eq. 3.1c, the constraints vector of the general multiple-interval, multiple-phase trajectory optimisation problem is required to be defined as a single nonlinear function with the argument being the vector of decision variables. Hence, analogous to the formulation of the decision variables, the total constraint vector of a multiple-phase problem is defined as,

() () 퓒(퓩) = 퓬 , … , 퓬 , 퓬 , (4.21) where each 퓬(), {푝 = 1,…, 푃}, is given by

() ⎡ 흃 ⎤ ⎢ ⋮ ⎥ () ⎢ 흃 () ⎥ ⎢ 풙 ⎥ () ⎢ 휸풄 ⎥ () ⎢ ⋮ ⎥ 퓬 = ⎢ () ⎥ , (4.22) 휸 ⎢ 풄 () ⎥ 풄 ⎢ ⎥ 휸() ⎢ 풃 ⎥ ⎢ ⋮ ⎥ ⎢휸() ⎥ 풃 () ⎣ 풃 ⎦

() () () and where each vector 흃 ∈ℝ , 푖 = 1,…, 푁풙 , is a column of the defect constraints matrix, 횵(). This matrix is defined as per Eq. 4.11, but is presented here in vectorised form

() () 푡 − 푡 () () 횵() = 푫()퐗() − 퐅() ∈ ℝ × 풙 . (4.23) 2

() The vectors 휸() ∈ℝ , 푖 = 1,…, 푁(), are the columns of the path constraint matrix 풄 evaluated at each of the collocation points, given by

() () ⎡ 휹풄 퐗 , 퐔 , 푡 ⎤ () () () ⎢ ⎥ × 횪퐜 = ⋮ ∈ℝ . (4.24) ⎢ () () ⎥ 휹 퐗 , 퐔 , 푡 () ⎣ 풄 () () ⎦

The vectors 휸(), 푖 = 1,…, 푁(), are the boundary constraints which are evaluated at the initial 풃 and terminal time for each phase.

The vector of the linear linkage constraints, 퓬, is appended to the end of the sequence of phase-dependent constraints, 퓬(), {푝 = 1,…, 푃}, and is defined as per Eq. 4.3b. Expressed in vectorised form,

() () ⎡ 푡 − 푡 ⎤ ⎢ () () ⎥ 퐗 − 퐗 () ⎢ ⎥ () ∑ 풙 퓬 = ⎢ ⋮ ⎥ ∈ℝ (4.25) ⎢ () () ⎥ 푡 − 푡 ⎢ ⎥ 퐗() − 퐗() ⎣ ()⎦

The upper and lower bound vectors for the constraints, 퓒 and 퓒, are defined in the exact same way as 퓒(퓩). In the context of the NLP, the constraint vector and its bounds form Eq. 3.1c.

4.4 Chapter Summary

This chapter has outlined the discrete formulation of a general multiple-interval, multiple-phase trajectory optimisation problem using the Radau pseudospectral method of orthogonal collocation. Subsequently, the specific NLP that is passed to IPOPT in order to solve the trajectory optimisation problem has been derived. To summarise, Eq. 4.1 is the continuous time formulation that must be passed to the software by a user for the problem they wish to solve. Using the RPM, this form is discretised as follows: the system dynamics are collocated to the discrete form of Eq. 4.11; the path, boundary and linkage constraints are transcribed into the form of Eq. 4.3b; and, the Bolza form cost is transcribed into the form of Eq. 4.12. Finally, to construct the NLP that takes the required form of Eq. 3.1, the constraint functions are all converted to the form of Eq. 4.21, with the decision variables concatenated into a single vector as per Eq. 4.15. The Bolza form cost is still expressed as Eq. 4.12. IPOPT then solves the NLP defined by Eq. 4.12, Eq. 4.15 and Eq. 4.21, combined with the appropriate upper and lower bound vectors. The solution of this NLP is the numerical solution of the general continuous time multiple-interval, multiple-phase trajectory optimisation problem.

Chapter 5 Extensions to Software for Tractability

This chapter outlines two extensions to the software that will assist in obtaining accurate solutions and minimising computation times as problems grow in size. This links back to the second and third aims of this project, as listed in Section 1.2. The extensions to the software are: (i) an adaptive grid refinement algorithm, and (ii) exploitations of sparsity in order to efficiently compute the constraint Jacobian and Lagrangian Hessian of the NLP.

5.1 Grid Refinement Algorithm

The ph-method of grid refinement implemented within the software is now formally outlined. As the state variables have a unique function approximation provided by the system dynamics equation, the core idea of the algorithm is to obtain a discretisation error estimate using two state approximations, one from an 푛-th order approximation and another from an (푛 + 1)-th order approximation.

While the system dynamics were collocated using a differential form in Eq. 4.11, for the error estimate it is more convenient to use an integral form of the RPM as it leads to an explicit algebraic equation for the state approximation (Garg et al., 2017; Patterson et al., 2014). This integral form is obtained by integrating both sides of the dynamics in Eq. 4.3b, and is given as

() ( ) ( ) 푡 푝 − 푡 푝 () (,) () (,) 푓 0 (,) ( ) (,) (,) (,) 퐗 휏 − 퐗 휏 = 푨 풇 휏 , 퐗 , 퐔 (5.1) 2 ,

( ) ( ) ( ) × where 푨 , ∈ℝ is known as the LGR Integration matrix (Garg et al., 2017). Here, 푘 represents the grid interval number, 푝 represents the phase number and 푚 represents the

() collocation point index where 푚 ∈ 1,…, 푛 , as per Section 4.2.

The refinement algorithm is executed sequentially over each phase of a multiple-phase problem. Therefore, only a single-phase problem needs to be considered to define the algorithm. Within this chapter, superscripts and subscripts containing the variable 푘 will be used to represent information in the 푘-th grid interval.

Assume that a single-phase trajectory optimisation problem has been solved using the RPM over a set of grid intervals 풮 = [푇, 푇], 푘 = 1,…, 퐾, with 푁 LGR collocation points in interval 풮 such that 휏() = 푇 and 휏() = 푇 . The notation and definition of these variables is the same as Chapter 4. To compute the discretisation error, firstly let {휏̂(),…, 휏̂()} be a new set of LGR collocation points such that 푀 = 푁 +1, 휏̂() = 푇 and 휏̂() = 푇 . The values of the state approximations at the new set of LGR points can be found using Eq. 4.4 and are denoted 퐗 휏̂(),…, 퐗 휏̂(). Note that these are obtained by simply interpolating the state approximations from the set of 푁 LGR points to the new set of 푀 points. Hence, this is still an 푁-th order estimate of the state since the original approximations were made using 푁 LGR points. To obtain the (푁 + 1)-th order state estimate, suppose that the control is also approximated at the new set of 푀 LGR points using Eq. 4.9, and denote these approximations as the set 퐔 휏̂(),…, 퐔 휏̂(). Thus, by applying Eq. 5.1, the state can be approximated as

푡푓 − 푡0 퐗 휏̂() = 퐗 휏̂() + 푨() 풇 휏̂(), 퐗 휏̂(), 퐔 휏̂(), (5.2) 2 , where 푗 = 2,…, 푀 +1. Note that Eq. 5.2 is a state approximation based on an interpolation of () the control, and the state is then obtained through integration, whereas the set 퐗휏̂ ,

푖 = 1,…, 푀, is simply an interpolation of the state approximation. The difference between these two provides an estimate of the discretisation error as equality cannot possibly be achieved unless the control approximation is correct, and the grid is sufficiently accurate (Patterson et al., 2014). The absolute error in each state is obtained as

() () () 푬휏̂ = 퐗휏̂ − 퐗휏̂ , 푙 = 1, … , 푀 + 1. (5.3)

The relative error in each state, which is the error within a grid interval 풮, is given as

() 푬휏̂ 풆 휏̂() = , () () (5.4) 1 + max max 퐗 휏̂ , 퐗̇ 휏̂

where 푙, 푗 = 1,…, 푀 +1 and 푖 = 1,…, 푁풙. Note that the operations in Eq. 5.4 are carried out element wise as both the numerator and denominator are vectors of size ℝ×풙 (Betts, 2010).

The way in which a grid is refined by the algorithm is based upon the maximum relative error, 푒 . The maximum relative error across all states in interval 푆 is given by

() 푒 = max 풆휏̂ , 푖 = 1, … , 푁풙. (5.5)

Now, suppose that 푒 is greater than some specified discretisation error tolerance 휖 ∈ℝ. In this situation, the grid is required to be refined. As per Section 2.7, either the order of the approximating polynomial can be increased (add more LGR collocation points) or the grid interval can be subdivided. To ensure the order of the polynomial does not grow too large, let

푁 be a user-specified upper limit on the number of LGR points within a grid interval. Patterson et al. (2014) has shown that in order to satisfy a given error tolerance of 휖, for a calculated 푒 the polynomial order must be increased by 푃 where

푒 푃 = log . (5.6) 휖

If the new polynomial order, which is equal to 푁 + 푃, is above 푁, then the interval must be subdivided. Again, to ensure the problem does not grow too large, let 퐺 be a user- specified maximum on the number of subdivisions that can be applied to any grid interval.

Furthermore, let 푁 be a user-specified lower limit on the number of LGR points within a grid interval. Each new subdivided grid interval is constructed with 푁 LGR points, ensuring that the full range of allowable LGR points from 푁 to 푁 can be used with the refinement process. The total number of subdivisions, 퐵, for the 푘-th interval is given by

푁 + 푃 퐵 = min 퐺, max , 2. (5.7) 푁

The expression in Eq. 5.7 ensures that the total number of new collocation points in the refined grid is the same regardless of whether the polynomial order was increased or the grid interval was subdivided (Patterson et al., 2014).

A summary of the grid refinement algorithm is presented in Algorithm 5.1. This algorithm is executed over every phase of a multiple-phase problem.

Algorithm 5.1: Adaptive ph-method of grid refinement.

Data: 퐾 = Number of grid intervals, 휖 = Grid tolerance, 푁 = maximum number of LGR

points in a grid interval, 푁 = minimum number of LGR points in a grid interval,

퐼 = maximum number of grid refinement iterations. 1. Set grid iteration number to 퐼 =0 and construct initial grid. 2. Solve NLP on current grid. 3. for 푘 = 1: 퐾

Compute relative error 풆.

Compute maximum relative error 푒 .

if 푒 > 휖:

Compute 푃 and 푁 + 푃

if 푁 + 푃 ≤ 푁:

Increase number of LGR collocation points in interval 푘 to 푁 + 푃

else:

Compute 퐵 and subdivide interval into 퐵 intervals with 푁 LGR points in each subinterval.

end

end end

4. Set 퐼 ∶= 퐼 +1 and return to Step 2 if 퐼 ≤ 퐼, else the exit algorithm.

5.2 Exploiting Sparsity in the Radau Pseudospectral Method

This subsection defines the sparsity patterns for the NLP’s constraint Jacobian and Lagrangian Hessian. An efficient method to calculate the derivatives by exploiting sparsity is also outlined.

5.2.1 Sparsity Patterns of the RPM Constraint Jacobian and Lagrangian Hessian Matrices

When solving the NLP resulting from the RPM, defined in Section 4.3, large sparse constraint Jacobian and Lagrangian Hessian matrices arise. For typical applications, these matrices are up to 98% sparse and exploitation of this sparsity significantly improves the computational () efficiency of NLP solvers (Betts, 2010). The constraint Jacobian, 퓙 , and Lagrangian

() Hessian, 퓗푳 , resulting from the RPM are defined in Eq. 5.8 and Eq. 5.9, respectively.

퓙() = ∇퓬()픃(), (5.8)

() () () 풙 풄 퓗() = 휎풇()픃() + 흀() 흃() + 흀() 휸() + 휈() 휸(), (5.9) 푳 , , 풄 풃풓

() () () where 휎 ∈ℝ is a Lagrange multiplier for the objective function, 풇 ∈ℝ, and 흀, , 흀, ∈ℝ are column vectors containing the Lagrange multipliers associated with the defect and path () constraints, and 휈 ∈ℝ are Lagrange multipliers associated with the boundary constraints. As the phase linkage constraints are strictly linear constraints, they do not contribute to the Hessian.

The NLP solver IPOPT must be used in one of two modes: (i) first derivative mode, or (ii) second derivative mode. These modes correspond to the order of the derivatives that a user supplies to the algorithm. That is, if approximations of only the objective gradient and constraint Jacobian are supplied by the user, then this is referred to as using IPOPT in first derivative mode. On the other hand, if approximations of the Lagrangian Hessian are also supplied, then this is known as using IPOPT in second derivative mode. When IPOPT is used in first derivative mode, the algorithm internally constructs an approximation of the Lagrangian Hessian using limited-memory, dense quasi-Newton techniques by default (Wächter and Biegler, 2006). The negative impact of this dense quasi-Newton approximation can be considerable, because dense representations of an 푛 × 푛 matrix grow by 푛, whereas sparse representations grow linearly with the number of non-zero elements (Wächter and Biegler, 2006). 50

IPOPT can take advantage of sparsity by storing Jacobian and Hessian matrices in sparse formats (Wächter and Biegler, 2006). However, in order for this to occur, the user must provide sparsity patterns for the Jacobian and the Hessian, and these patterns must remain constant for the entire optimisation procedure. Hence, the provided sparsity patterns must indicate the row and column index of entries that could be non-zero at any time during the optimisation, and not just those that are non-zero at the initial point. Fortunately, the LGR collocation method possesses well-defined sparsity patterns for both the constraint Jacobian and Lagrangian Hessian matrices (Agamawi and Rao, 2018). For multiple-interval, multiple-phase problems, only the size of the matrices change with the number of collocation points and grid intervals, not the overall structure. A graphical representation of the Jacobian sparsity pattern for a single- phase problem is shown in Fig. 5.1 and this is extended to a multi-phase representation in Fig 5.2. Similar representations for the Lagrangian Hessian of Eq. 5.9 are shown in Fig. 5.3 and Fig 5.4, respectively. Non-zero elements are indicated with large black circles.

Figure 5.1: Single-phase constraint Jacobian sparsity pattern for the RPM. Each block of defects refers to a set of 푛 constraints associated with a state, e.g. Defect 1 is associated with State 1. When defects are derived at each discretistaion point with respect to their associated state, a dense block is formed. When derived with respect to other variables, diagonal blocks are formed. 51

Figure 5.2: Multiple-phase constraint Jacobian sparsity pattern for the RPM. This is formed as a block diagonal concatenation of single-phase sparsity patterns plus the additional state and time linkage constraints which are appended on the bottom. The total number of rows corresponds to the number of constraints and the total number of columns corresponds to the number of variables for the problem.

State 1 State 푁 Control 1 Control 푁 푡푡 State 1 푁 Control State 1 푁 Control 푡

푡

Figure 5.3: Single-phase Lagrangian Hessian sparsity pattern for the RPM. The diagonal blocks all have 푛 non-zero diagonal terms, where 푛 is number of LGR points.

Phase P Phase2 1 Phase Figure 5.4: Multiple-phase Lagrangian Hessian sparsity pattern for the RPM. 53

Note that since the Lagrangian Hessian is a symmetric matrix, IPOPT only requires that the sparsity of the bottom triangular region be provided. Also note that there are no linkage constraints involved in the Hessian, as these are purely linear constraints. Within the software, the above sparsity patterns are automatically generated simply by knowing the number of nodes, grid intervals, phases, states, controls, path constraints and boundary constraints for a problem. Lastly, it is also worth noting that the structures passed into IPOPT are typically a conservative estimate of the sparsity, as elements that have been indicated as potentially non-zero, could be zero-valued depending on the problem.

5.2.2 Exploiting Sparsity to Compute Derivatives

There are two approaches to construct the objective gradient, constraint Jacobian and Lagrangian Hessian matrices of a multiple-interval, multiple-phase trajectory optimisation problem. The first of these is the obvious, and common, approach of deriving the NLP functions of Eq. 3.1 to obtain the required derivatives (Betts, 2010). The second approach is to derive the optimal control functions of Eq. 4.3, and then populate the objective gradient, constraint Jacobian and Lagrangian Hessian matrices with the derivatives in the appropriate locations. This second approach was developed by Betts and Huffman (1999) for local direct collocation methods and then expanded upon for pseudospectral methods by Patterson and Rao (2012a).

Whilst perhaps not obvious initially, the sparsity patterns in Fig. 5.1 through Fig. 5.4 reveal significant potential pitfalls of the first approach. Figure 5.1 shows that most constraints are only functions of a subset of the total set of decision variables. For example, the boundary constraints are only functions of the initial and terminal state as well as the initial and terminal time. Furthermore, Fig. 5.2 shows that constraint functions are not dependent on variables from different phases. If the constraint Jacobian is obtained by deriving the NLP functions of Eq. 3.1, many unnecessary calculations are carried out as each function in the constraint vector must be derived with respect to the entire set of decision variables (Betts and Huffman, 1999). This includes variables from other phases of the problem. A similar situation arises for the Lagrangian Hessian evaluation.

The second approach exploits the sparsity patterns because this approach ensures derivative evaluations are only carried out for potential non-zero terms. This approach requires significantly less computations and drastically improves the efficiency of the solver (Betts and Huffman, 1999). This is because Jacobian and Hessian evaluations are typically the most expensive operations when solving an NLP (Patterson and Rao, 2012a).

These two approaches were compared by Patterson and Rao (2012a) on a trajectory optimisation problem using finite-differencing. IPOPT was used as the NLP solver. Table 5.1 provides a summary of the results.

Table 5.1: Comparison between two approaches to compute Jacobian and Hessian matrices (Patterson and Rao, 2012a). IPOPT CPU IPOPT CPU IPOPT No. of No. of Time (sec). Time (sec). Test No. of No. of Derivative Grid Collocation Approach 1 – Approach 2 – Case Variables Constraints Mode Intervals Points Derive NLP Exploit Functions Sparsity 1 1st 16 64 390 321 36.2 1.7 2 1st 32 128 774 641 57.9 1.9 3 1st 64 256 1542 2561 133 3.5 4 1st 128 512 3078 2561 435 7.6 5 1st 256 1024 6150 5121 1051 13.7 6 1st 512 2048 12294 10241 2246 27.1 7 2nd 16 64 390 321 44.5 0.83 8 2nd 32 128 774 641 100 1.3 9 2nd 64 256 1542 2561 263 2.5 10 2nd 128 512 3078 2561 708 4.9 11 2nd 256 1024 6150 5121 2058 11 12 2nd 512 2048 12294 10241 5871 23.1

As the results show, the impact of exploiting sparsity is dramatic. With an increase in problem size, the computational burden grows several orders of magnitude when deriving the NLP functions. For example, Test Case 6 has 32 times the number of grid intervals and variables than Test Case 1, but the computation time grows by a factor of 62. In comparison, by exploiting sparsity, the computation time only grew by a factor of 16 – and the times themselves were 20 to 80 times less than the times from Approach 1.

The results also show that the evaluation of the Hessian induces substantial computational burden for Approach 1, as when IPOPT was used in second derivative mode it took longer for every test case than when it was used in first derivative mode. However, by exploiting sparsity, IPOPT solved the NLP more efficiently in second derivative mode. This implies that the evaluation time for the Hessian reduced drastically by exploiting sparsity. In subsequent test cases (not shown here), Approach 2 with finite-differencing was compared against Approach 1 using AD and the former was again seen to be the more efficient method (Patterson and Rao, 2012a).

The software package built within this thesis exploits sparsity to compute the derivatives. Explicit expressions for the RPM’s objective gradient, constraint Jacobian and Lagrangian Hessian have been derived in Patterson and Rao (2012a) and in Agamawi and Rao (2018). Note that IPOPT does not require a sparse representation for the grad vector of the objective function, as this is simply a single row vector that requires little memory allocation and its computation is typically inexpensive (Wächter and Biegler, 2006).

Chapter 6 Software Architecture and Usage

This section describes the algorithmic flow of the trajectory planner software with reference to Figure 6.1. The software has been given the name Pseudospectral OPTimiser (풫풪풫풯) and will be referred to as such within the rest of the document. An execution of 풫풪풫풯 is carried out as follows. Firstly, the user provides the problem to be solved accompanied by an initial guess and

Figure 6.1: Flowchart of Pseudospectral OPTimiser (풫풪풫풯).

the problem bounds. The problem setup from the user is then validated before the core of the algorithm is entered. The initial grid upon which the NLP will be solved is then set up before the RPM is used to transcribe the continuous time trajectory optimisation. The initial guess and the upper and lower bounds on the decision variables and the constraints are then processed in order to be passed into IPOPT. Following this, problem scaling is carried out if the user has requested it using the in-built scaling regime. The various options for IPOPT, such as the derivative calculation method, are then set up either to the user’s specifications or using default options. Subsequently, the NLP is solved using IPOPT. An analysis of the solution is then carried out to determine the discretisation error on the current grid. If a user-specified error tolerance is satisfied in every grid interval, the algorithm terminates and the solution is outputted. Otherwise, the grid is refined and the optimisation step is repeated on the new grid in order to achieve the desired discretisation accuracy. The initial guess on the new grid is set equal to the solution obtained on the previous grid iteration.

Chapter 7 Usage of Software and Constructing a Problem

This chapter serves as a condensed user’s guide for 풫풪풫풯. A complete, stand-alone user’s guide is provided in Appendix A. 풫풪풫풯 is distributed under the MIT License and is available from the following repository: github.com/InderpreetMetla/popt.

7.1 Constructing a Problem

The call to 풫풪풫풯 is deceptively simple. A problem is passed and solved by the software using the syntax

[solution, plotdata] = popt(problem), where problem is a user-defined MATLAB structure that contains every piece of information about the problem to be solved. The output solution is a MATLAB structure that contains the solution and all information outputted by IPOPT regarding the optimisation procedure, such as CPU time and the number of iterations. The second output, plotdata, is a MATLAB structure that contains the solution interpolated at five times the number of LGR points in order to streamline the process of creating plots. This can be modified by the user through an options field that interfaces with 풫풪풫풯.

The problem struct has nine possible fields, five of which are optional. The four required fields in problem are:

 problem.name: A string with no blank spaces that provides a name for the problem.  problem.funcs: A structure with five fields where each field is a user-defined function handle for the dynamics, path constraints, boundary constraints, path objective and boundary objective.  problem.bounds: A structure that contains the upper and lower bounds on the states, controls, time and constraints. This structure is phase dependent.  problem.guess: A structure that contains the guess for the state, control and time trajectories. This structure is phase dependent. 59

The five optional fields are:

 problem.derivatives: A structure with four fields (Table 7.1 provides options and default settings). o .method: Specifies the derivative calculation scheme to be used. o .order: Specifies the derivative order to be used by IPOPT. o .first: If using finite-differencing or complex-step, this field can be used to set the step-size for the gradient and Jacobian approximations. o .second: If second order derivatives are desired to be supplied to IPOPT by 풫풪풫풯 rather than using a limited-memory, quasi-Newton approximation, this field can be used to set the step-size for the Hessian approximation. The Hessian is only approximated by 풫풪풫풯 using forward-differencing.

 problem.grid: A structure with three fields (Table 7.2 provides default settings). o .tol: A constant tolerance for the desired accuracy on every grid interval. o .max_refine: The maximum number of grid refinement iterations. o .phase(p): A structure with one field where (p) represents the 푝th phase of the problem. . .nodes: A structure with three fields:  .initialgrid: A row vector that contains the number of nodes in each of the intervals on the initial grid. The size of the vector designates the number of grid intervals in the initial grid. All intervals are equally spaced.  .lb: The lower bound on the number of LGR points in each new grid interval through the refinement process.  .ub: The upper bound on the number of LGR points in each new grid interval through the refinement process.

 problem.auxdata: A structure that contains any auxiliary data used by any of the functions in problem.funcs. This must be used to pass data between functions as it is also used by IPOPT. For each set of data, a new field is to be created.

 problem.autoscale: A string that specifies whether or not to use the in-built automatic scaling scheme. Options are either ‘on’ or ‘off’ and default is ‘off’.

 problem.options: A structure that allows the user the opportunity to modify IPOPT settings. Commonly modified settings are shown in Table 7.3.

Table 7.1 presents the options and default settings for the problem.derivatives structure.

Table 7.1: Options and default settings for problem.derivatives. Field Options Default

‘fd’ or ‘FD’ = forward-difference .method ‘cs’ or ‘CS’ = complex-step ‘fd’ ‘ad’ or ‘AD’ = auto-differentiation

.order ‘1’ or ‘2’ ‘1’

.first.stepsize.gradient A positive number 3 × 10

.first.stepsize.jacobian A positive number 3 × 10

.second.stepsize.hessian A positive number 1.75 × 10

Table 7.2 presents the default settings for the problem.grid structure.

Table 7.2: Options and default settings for problem.grid. Field Options Default

.tol A positive number 1 × 10

.max_refine A positive integer 10

Row vector of length equal to the number of .phase(p).nodes. desired grid intervals, with each element 4*ones(1,10) initialgrid equal to number of desired LGR points.

.phase(p).nodes.lb A positive integer 3

.phase(p).nodes.ub A positive integer 10

Table 7.3 presents commonly modified settings for IPOPT. Additional settings can be found in Kawajir et al. (2015).

Table 7.3: Options and default settings for problem.options. Field Options Default

.ipopt.linear_solver ‘mumps’ or ‘ma57’ ‘mumps’

.ipopt.max_iter A positive integer 3000

.ipopt.tol A positive number 1 × 10

7.1.1 Creating the problem.funcs Struct

There are five fields in the problem.funcs structure, with each field being a user-defined function handle. All five fields are mandatory even if the user’s problem does not require them. The fields are

 .Dynamics: Function handle for the system dynamics,  .PathObj: Function handle for the Lagrange (path) objective,  .PathCst: Function handle for the path constraints,  .BndObj: Function handle for the Mayer (boundary) objective, and

 .BndCst: Function handle for the boundary constraints.

Note that there is no requirement for the user to supply a MATLAB function for the linkage constraints. This is because the required function is constructed by 풫풪풫풯 automatically.

7.1.2 Syntax for Dynamics, Path Objective and Path Constraint Functions

The dynamics, path objective and path constraint functions are all set up in a similar manner to each other. A separate function is required for each and the syntax to define the functions is:

function output = FUNCTION_NAME(t,x,u,auxdata) where FUNCTION_NAME is replaced with whatever the user desires. The variable t is a column vector of length 푁(), where 푁() is the number of LGR collocation points in phase 푝, x is a () matrix of size 푁 × 푁, where 푁 is the number of states for the problem, and u is a matrix

() () () of size 푁 × 푁 , where 푁 is the number of controls in phase 푝. The auxdata is a struct that contains the information provided by the user in problem.auxdata. Note that the number of states for a problem must be constant across phases. Hence, the phase dependency notation has been dropped for the term 푁.

The output of these three functions, output, changes size depending on the function. For () the dynamics function it is a matrix of size 푁 × 푁, whereas for the path objective function it is a vector of length 푁() and for the path constraints function it is a matrix of size

() () () 푁 × 푁 , where 푁 is the number of path constraints in phase 푝.

In order to distinguish between phases, a field auxdata.iphase is automatically created for multiple-phase problems and this can be used with conditional if/else statements if the dynamics, path objective or path constraints change from one phase to the next.

Note: For problems that do not have a path objective, a MATLAB function must be supplied with the output set to zeros(size(t)). This syntax ensures that the output has correct dimensions and the derivatives are computed accurately. Similarly, if a problem does not have path constraints, a function must still be supplied with the output set to an empty array, [].

7.1.3 Syntax for Boundary Objective and Boundary Constraint Functions

The boundary objective and boundary constraint functions are both set up in a similar manner. A separate function is required for each and the syntax to define the functions is:

function output = FUNCTION_NAME(t0,tf,x0,xf,auxdata)

Here t0 and tf are scalars representing the initial and terminal time, respectively, and x0 and xf are both a row vector of length 푁 representing the initial and terminal state. The field auxdata is the same here as it was in Section 7.1.2.

The output of these two functions, output, again changes size depending on the function. For the boundary objective function, it is a scalar, whereas for the boundary constraint function () () it is a row vector of length 푁 , where 푁 is the number of boundary constraints in phase 푝.

Note: For problems that do not have a boundary objective, a MATLAB function must be supplied with the output set to zeros(size(t0)). This syntax ensures that the output has correct dimensions and the derivatives are computed correctly. Similarly, if a problem does not have boundary constraints, a function must still be supplied with the output set to an empty array, [].

7.1.4 Creating the problem.bounds Struct

The constant upper and lower bounds on the variables and constraints can be defined within the problem.bounds structure. This struct contains two fields: .phase(p), which is an array of 푃 structs where 푃 is the number of phases, and .link(휓), which is an array of 푃 −1 structs.

The .phase(p) array defines the constant bounds on the time, state and control variables as well as the path and boundary constraints. The 푝th element within this array of structs stores the following information:

 problem.bounds.phase(p).initialtime.lb = 푡 ∈ℝ  problem.bounds.phase(p).initialtime.ub = 푡 ∈ℝ  problem.bounds.phase(p).finaltime.lb = 푡 ∈ℝ  problem.bounds.phase(p).finaltime.ub = 푡 ∈ℝ

() ×  problem.bounds.phase(p).initialstate.lb = 풙ퟎ ∈ℝ

() ×  problem.bounds.phase(p).initialstate.ub = 풙ퟎ ∈ℝ

()  problem.bounds.phase(p).state.lb = 풙 ∈ℝ×

()  problem.bounds.phase(p).state.ub = 풙 ∈ℝ×

(푝) − 1×푁푥  problem.bounds.phase(p).finalstate.lb = 풙풇 ∈ℝ

(푝) + 1×푁푥  problem.bounds.phase(p).finalstate.ub = 풙풇 ∈ℝ

1×푁(푝)  problem.bounds.phase(p).control.lb = 풖− ∈ℝ 푢

1×푁(푝)  problem.bounds.phase(p).control.ub = 풖+ ∈ℝ 푢

(푝) − 1×푁푐  problem.bounds.phase(p).path.lb = 풄 = 풄푳 ∈ℝ (푝) + 1×푁푐  problem.bounds.phase(p).path.ub = 풄 = 풄푼 ∈ℝ (푝) − 1×푁푏  problem.bounds.phase(p).boundary.lb = 풃 = 풃퐿 ∈ℝ (푝) + 1×푁푏  problem.bounds.phase(p).boundary.ub = 풃 = 풃푼 ∈ℝ

The .link(휓) array defines the constant bounds on the linkage constraints. The 휓th element within this array of structs stores the following information:

() ×  problem.bounds.link(휓).lb = 휹 = 휹 ∈ℝ 휹

() ×  problem.bounds.link(휓).ub = 휹 = 휹 ∈ℝ 휹

It is worth noting that if a problem does not have path or boundary constraints, or is not a multi- phase problem, then the relevant field can be omitted or the lower and upper bounds can be set to the empty array. Furthermore, infinite valued bounds are not permitted and should instead be replaced with a sufficiently large number.

7.1.5 Creating the problem.guess Struct

The guess to initialise an optimisation is provided within problem.guess. This struct contains a .phase(p) field which is used to define the guess for the time, state and control in every phase of the problem. A guess for the time, state and control at the initial and terminal points of the trajectory must be provided at a minimum. The limit to the number of points used to define the initial guess is set by the number of LGR points in the initial grid. The same number of points must be used in the guess for the time, state and control. The format of the guess struct is:

(푝) 푔(푝)×1  problem.guess.phase(p).time = 푡푔푢푒푠푠 ∈ℝ

() () ×  problem.guess.phase(p).state = 풙 ∈ℝ

() () () ×  problem.guess.phase(p).control = 풖 ∈ℝ where 푔() is the number of points used to define the guess and 푔() ≥2.

7.2 Details for the Output of the Software

The two outputs from an execution of the software are the solution structure and the plotdata structure. The solution structure contains the following ten fields.

 solution.phase(p): An array of structs of length 푃 with three fields: o .time: The time solution at the discretisation points on the final grid iteration, o .state: The state solution at the discretisation points on the final grid iteration. o .control: The control solution at the discretisation points on the final grid iteration.

 solution.cost: The optimal objective value.

 solution.bndcost: The contribution from the Mayer objective to the total cost.

 solution.pathcost: The contribution from the Lagrange objective to the total cost.

 solution.status: Information regarding the reason IPOPT terminated.

 solution.nlp_iters: Number of IPOPT iterations.

 solution.f_evals: Number of IPOPT function evaluations.

 solution.cpu_time: A struct with two fields: o .grids: A vector containing IPOPT CPU time to solve the problem on each grid refinement iteration. o .total: The total CPU time for the entire optimisation procedure.

 solution.grid_iters: The number of grid refinement iterations.

 solution.grid_analysis: An array of structs that has length equal to the number of grid refinement iterations. This contains a history of the solution procedure with the optimal cost, trajectory solution, discretisation error and IPOPT information on each of the grid iterations.

The plotdata structure contains the same information as solution.phase(p), but the results are provided at five times the number of points in order to streamline the process of creating plots.

Chapter 8 Results: Implementation and Performance Evaluation on Example Problems

8.1 Overview of Example Problems

This section benchmarks the performance of 풫풪풫풯 against open-source and commercial solvers. Five examples from literature have been considered to showcase the range of problems that can be solved. Unless noted otherwise, all example problems were solved using MATLAB R2017b on a 3.5 GHz Intel Core i5-7600 desktop PC with 16 GB of RAM running 64-bit Windows 10. The example problems and their features have been provided in Table 8.1. The software used to benchmark 풫풪풫풯 have also been listed.

Table 8.1: Example problems and their features.

Example Objective Multi- Software Constraints Special Features Problem Form Phase Compared

N/A LQR Lagrange - Dynamics No - Infinite-Horizon (Exact solution)

- 풫풪풫풯 Bryson - Dynamics Lagrange No N/A - 픾ℙ핆ℙ핊 − 핀핀 Denham - Path Constraints - OptimTraj

- Dynamics - Bang-Bang control - 풫풪풫풯 Free-Flying Lagrange - Path Constraints No - Absolute value in - 픾ℙ핆ℙ핊 − 핀핀 Robot - Boundary Constraints Lagrange cost - 핊핆ℂ핊

Lee- - Lagrange cost - 풫풪풫풯 - Dynamics Ramirez Bolza No contains rate of - PROPT - Boundary Constraints Bioreactor change of control - 풫풮풪풫풯

- Dynamics Goddard - Path Constraints - 풫풪풫풯 Mayer Yes - Singular Arc Rocket - Boundary Constraints - 픾ℙ핆ℙ핊 − 핀핀 - Phase Linkages

8.2 Example 1 – Continuous Time Infinite-Horizon LQR

Consider the following continuous time infinite-horizon Linear Quadratic Regulator (LQR) problem from Kirk (2012). We seek to minimise the Lagrange type performance objective

1 1 (8.1) 퐽 = 푥 (푡) + 푥 (푡) + 푢 (푡) 푑푡, 2 4 subject to the second-order dynamic constraints

(8.2a) 푥̇(푡) = 푥(푡)

(8.2b) 푥̇(푡) = 2푥(푡) − 푥(푡) + 푢(푡) and the initial condition

(8.3) 푥(0) = −4, 푥(0) = 4.

The analytical approach to solving this problem is to solve the algebraic Riccati equation (Lancaster and Rodman, 1995). This provides a state feedback gain matrix, 퐊, which gives the optimal feedback control law. The analytical solution obtained by Kirk (2012) is

퐱∗(푡) = exp([퐀 − 퐁퐊]푡) 퐱(0) (8.4a)

퐮∗(푡) = −퐊퐱(푡) (8.4b) where

0 1 0 퐀 = , 퐁 = , 퐊 = [4.82 2.56]. (8.4c) 2 −1 1

To solve this problem using 풫풪풫풯, the semi-infinite domain 푡 ∈ [0,∞) of the infinite- horizon LQR problem must be dealt with. Fahroo and Ross (2008) developed an approach that transforms this domain to the closed interval 휏 ∈ [−1,1] where evaluations at 휏 =1 are performed in the sense of the limit 푡 →∞. The transformation posed by Fahroo and Ross (2008) is given by Eq. 8.5.

휏 + 1 푑푡 2 푡 = ; = (8.5) 휏 − 1 푑휏 (1 − 휏)

As the RPM does not include the terminal point as a collocation point, the singularity at 휏 =1 is avoided. However, as stated in Section 4.2.1, the state is still approximated at 휏 =1 within the NLP. Hence, a state approximation at the horizon 푡 →∞ is obtained. Using this transform, the problem can be formulated in 풫풪풫풯 as follows: Minimise the cost functional

2 1 1 (8.6) 퐽 = 푥 (휏) + 푥 (휏) + 푢 (휏) 푑휏, (1 − 휏) 2 4 subject to the second-order dynamic constraints

2 푥̇ (휏) = 푥 (휏), (8.7a) (1 − 휏)

2 푥̇ (휏) = 2푥 (휏) − 푥 (휏) + 푢(휏), (8.7b) (1 − 휏) and the initial condition

(8.8) 푥(−1) = −4, 푥(−1) = 4.

The computed solution can then be converted back to the domain [0,∞) using Eq. 8.5.

풫풪풫풯 was initialised with straight-line guesses between the initial and terminal time for both of the state variables, as well as the control. The discretisation error tolerance required to be satisfied was set to 휖 =1×10. Complex-step differentiation was used for the Jacobian approximation and finite-differencing was used for the Hessian approximation. Table 8.2 presents a summary of the solution procedure, and the solution is shown in Figs. 8.1 and 8.2 with a comparison against the exact solution.

Table 8.2: Summary of execution of 풫풪풫풯 for Infinite-Horizon LQR problem. Maximum Error Maximum Number of Grid IPOPT CPU Time (sec) Against Exact Solution Discretisation Error Iterations (15 run average)

푥 ≔ 3.47 × 10 푥 ≔ 6.19 × 10 7.22 × 10 5 0.1480 푢 ≔ 5.53 × 10

Figure 8.1: Comparison between the solution for the Infinite-Horizon LQR problem from 풫풪풫풯 and the exact solution by Kirk (2012) on the closed domain [-1,1].

Figure 8.2: Comparison between the solution for the Infinite-Horizon LQR problem from 풫풪풫풯 and the exact solution by Kirk (2012) on the semi-infinite time domain.

The state and control approximations obtained by 풫풪풫풯 can be seen to be indistinguishable from the exact solution. This example validates the accuracy of 풫풪풫풯 and has demonstrated the ability of 풫풪풫풯 to solve infinite-horizon optimal control problems. This is illustrative as pseudospectral solvers which choose not to collocate at the Radau points, such as PROPT and 풫풮풪풫풯, are not able to solve infinite-horizon problems. Furthermore, this problem has also validated the accuracy of the Jacobian and Hessian approximations carried out by 풫풪풫풯. The code to solve this problem using 풫풪풫풯 is provided in Appendix B.

8.3 Example 2 – Bryson Denham

The following optimal control problem was first posed by Bryson et al. (1963) and is a common problem used to benchmark optimal control software (Leek, 2016). It is a state-constrained double integrator problem and is formulated as follows: Minimise the cost functional

1 퐽 = 푢 (푡) 푑푡, (8.9) 2 subject to the dynamics

푥̇(푡) = 푣̇(푡), (8.10a)

푣̇(푡) = 푢(푡), (8.10b) where 푥(푡) is position, 푣(푡) is velocity and 푢(푡) is the control variable. This cost is also subject to the following boundary conditions and the state path constraint,

푥(푡 = 0) = 푥(푡 = 1) = 0, 푣(푡 = 0) = −푣(푡 = 1) = 1, (8.11a)

0 ≤ 푥(푡) ≤ 푙 . (8.11b)

8.3.1 Comparison of 퓟퓞퓟퓣 and 픾ℙ핆ℙ핊 − 핀핀

The problem has been solved using 풫풪풫풯 and the commercial solver 픾ℙ핆ℙ핊 − 핀핀 (which also uses LGR collocation) for the case where 푙 = 1/9. Both 풫풪풫풯 and 픾ℙ핆ℙ핊 − 핀핀 were initialised with four intervals in the initial grid and eight collocation points in each interval. The discretisation error tolerance to satisfy was set to 휖 =1× 10. Finite-differencing was used with the in-built automatic problem scaling routine turned on. 픾ℙ핆ℙ핊 − 핀핀 implements a scaling routine similar to 풫풪풫풯’s with the difference being that 픾ℙ핆ℙ핊 − 핀핀 also scales the constraint functions, whereas 풫풪풫풯 only scales variables. An overview of the results from 풫풪풫풯 and 픾ℙ핆ℙ핊 − 핀핀 is shown in Table 8.3.

Table 8.3: Comparison of solution from 풫풪풫풯 and 픾ℙ핆ℙ핊 − 핀핀. Maximum IPOPT CPU Time Number of Grid Computed Solver Discretisation (sec) Iterations Objective Value Error (15 run average)

풫풪풫풯 4.760 × 10 5 0.5770 3.99999964028

픾ℙ핆ℙ핊 − 핀핀 5.939 × 10 4 0.6096 3.99999964025

From the results in Table 8.3, it can be seen that 풫풪풫풯 requires one extra grid iteration to achieve the required discretisation accuracy. Despite this, the CPU time is smaller, albeit by a relatively insignificant amount. The optimal objective values are seen to be identical to 10 decimal places. To further demonstrate the solution accuracy, the 풫풪풫풯 solutions have been compared with the analytical solution derived by Bryson et al. (1963) in Fig. 8.3.

Figure 8.3: Comparison of solution for the Bryson-Denham problem from 풫풪풫풯 and the exact solution by Bryson et al. (1963). The clustering of the nodes towards the switch locations in the control showcases the effectiveness of the grid refinement algorithm.

The maximum error between the computed trajectory from 풫풪풫풯 and the exact solution across the states and the control is 3.737 × 10. This is compared with a maximum error of 3.248 × 10 from 픾ℙ핆ℙ핊 − 핀핀. Hence, both 풫풪풫풯 and 픾ℙ핆ℙ핊 − 핀핀 can be concluded to achieve a similar accuracy for the Bryson Denham problem. The code to solve this problem using 풫풪풫풯 is provided in Appendix C.

8.3.2 Comparison of 퓟퓞퓟퓣 and OptimTraj

The problem was also attempted to be solved using the open-source solver OptimTraj, but was not successful. OptimTraj was initialised with the same initial guess as 풫풪풫풯, but 32 grid points were specified in a single interval as it does not support multi-interval formulations or have grid refinement capabilities. Even by increasing the number of grid points to 200, OptimTraj was still not successful at solving this problem as fmincon converged to an infeasible point after just 30 NLP iterations.

This problem has demonstrated that 풫풪풫풯 is a superior open-source solver than OptimTraj. The primary reasons why OptimTraj failed to successfully solve this problem were its choice of collocation method, which is a low order direct collocation method that uses trapezoidal quadrature, and because it does not have an in-built grid refinement scheme. A comparison between the two solutions is shown in Fig. 8.4. As can be seen, the control trajectory computed by OptimTraj did not converge and exhibits steep gradients. The lack of grid refinement algorithm in OptimTraj means that the collocation points are all equally spaced along the time axis. Hence, in order to resolve areas with steep gradients, an extremely large number of collocation points are required, and this imposes significant computational burden.

Figure 8.4: Comparison of solution for the Bryson-Denham problem from 풫풪풫풯 and OptimTraj.

As mentioned above, OptimTraj was unable to solve this problem even with 200 grid points. To further highlight the superior performance of 풫풪풫풯 compared to OptimTraj, this problem was initialised with a single grid interval containing just five points. 풫풪풫풯’s then required 7 grid refinement iterations and 0.77 seconds to yield a solution with a maximum discretistaion error of 1×10.

8.4 Example 3 – Free-Flying Robot

The following optimal control problem has been taken verbatim from Sakawa (1999) and Betts (2010). This problem computes the optimal thrust for a symmetrically shaped robot equipped with a twin-jet thrust mechanism. The objective is to minimise fuel consumption whilst satisfying a boundary condition on the motion. The robot is modelled with six states:

 푥, the horizontal position of the centre of gravity,

 푥, the vertical position of the centre of gravity,

 푥, the angular direction of thrust,

 푥, the horizontal velocity,

 푥, the vertical velocity, and

 푥, and the angular velocity.

The thrust from the two jets is denoted 푇 and 푇. These thrusts are the control inputs for the system. The period of motion is restricted to a 12 second interval giving the following objective function to minimise

(8.12) 퐽 = (|푇| + |푇|) 푑푡 this is subjected to the following dynamic constraints

(8.13a) 푥̇ = 푥

(8.13b) 푥̇ = 푥

(8.13c) 푥̇ = 푥

(8.13d) 푥̇ = (푇 + 푇)cos(푥)

(8.13e) 푥̇ = (푇 + 푇)sin(푥)

(8.13f) 푥̇ = 훼푇 − 훽푇

where 훼 = 훽 = ∈ℝ with 푑 being the distance from the centre of gravity to the jets, 푚 is the mass of the robot and 퐽 is the moment of inertia about the centre of gravity.

The objective is also subject to the following path constraints

(8.14) |푇(푡)| ≤ 1, |푇(푡)| ≤ 1, and the following boundary constraints

퐱(푡) = 퐱 = (−10, −10, π/2, 0,0,0), (8.15a)

(8.15b) 퐱푡 = 퐱 = (0,0,0,0,0,0).

What makes this problem illustrative is the presence of the absolute value functions within the objective, which results in the objective function having discontinuous derivatives. To handle this issue, a “trick” suggested by Betts (2010) can be used. The idea is to express each control input in terms of a positive and negative component,

(8.16a) 푇 = 푢 − 푢,

(8.16b) 푇 = 푢 − 푢, where each 푢 ≥ 0, 푖 = 1,2,3,4. Then, the integrand can be replaced with

(8.16c) |푇| = 푢 + 푢, |푇| = 푢 + 푢.

Equation 8.16 can be substituted into the appropriate terms in Equations 8.12, 8.13 and 8.14 to give the new formulation.

This problem has been solved using 풫풪풫풯 and 픾ℙ핆ℙ핊 − 핀핀. The solution from Betts (2010) obtained using 핊핆ℂ핊 has also been considered for comparison. The initial guess provided to 풫풪풫풯 and 픾ℙ핆ℙ핊 − 핀핀 for the states were straight lines that satisfy the boundary conditions, and the controls were all guessed as a constant zero-valued trajectory. 풫풪풫풯 and 픾ℙ핆ℙ핊 − 핀핀 were initialised with two grid intervals and five nodes in each interval to match the initial grid used in the 핊핆ℂ핊 solution. Derivatives were approximated using finite- differences in all three software. The required discretisation error was set to 휖 =1×10, which is the same as what was used in the 핊핆ℂ핊 solution. An overview of the results from each solver is presented in Table 8.4. Note that IPOPT CPU times for 풫풪풫풯 and 픾ℙ핆ℙ핊 − 핀핀 are 15 run averages. The code to solve this problem using 풫풪풫풯 is provided in Appendix D.

Table 8.4: Comparison of solution procedure from 퓟퓞퓟퓣, 픾ℙ핆ℙ핊 − 핀핀 and 핊핆ℂ핊 for Free-Flying Robot problem. Maximum Number of Grid IPOPT CPU Time Computed Solver Discretisation Error Iterations (sec) Objective Value

풫풪풫풯 8.617 × 10 14 20.21 7.9100992675

픾ℙ핆ℙ핊 − 핀핀 9.543 × 10 16 17.32 7.9096348044

핊핆ℂ핊 9.832 × 10 12 76.85* 7.9101546460

The state and control trajectories are shown in Figs. 8.5 and 8.6, respectively.

Figure 8.5: State trajectory for Free-Flying Robot problem from 풫풪풫풯.

Figure 8.6: Control trajectory for Free-Flying Robot problem from 풫풪풫풯.

* This computation time has only been reported here for completeness - it is not used for comparative purposes as the 핊핆ℂ핊 solution was obtained on a different machine. 76

The comparison in Table 8.4 shows that all three packages achieved near identical values for the objective. This validates the accuracy and optimality of the 풫풪풫풯 solution. It can also be seen that 풫풪풫풯 and 픾ℙ핆ℙ핊 − 핀핀 required similar computation times to solve this problem. Furthermore, 풫풪풫풯 was able to achieve a smaller discretisation error than 픾ℙ핆ℙ핊 − 핀핀, whilst requiring two less grid refinement iterations. This highlights the comparable performance of 풫풪풫풯 with 픾ℙ핆ℙ핊 − 핀핀. Based off this analysis, the performance of 풫풪풫풯 can be concluded to be on par with the other two solvers, both of which are commercial packages.

The control trajectories shown in Fig. 8.6 depict “bang-bang” behaviour, which is typical of problems where the control appears linearly in the differential equations (Betts, 2010). This showcases the capabilities of the grid refinement algorithm as the initial guesses for the control inputs were simply horizontal lines. The refinement algorithm was able to identify localised regions with high discretisation error, and then appropriately place a higher density of collocation points in order to achieve convergence. Figure 8.7 illustrates this further, showing a dense area of nodes around one of the switch locations in the bang-bang solution.

Figure 8.7: Distribution of nodes after grid refinement for the control input in the Free-Flying Robot problem. A dense node distribution is seen close to the switch.

8.5 Example 4 – Lee-Ramirez Bioreactor

Consider the following optimal control problem first posed by Lee and Ramirez (1994) and then later reconsidered by Balsa-Canto et al. (2001). The problem computes the optimal control for a fed-batch reactor used in chemical processes in order to maximise profitability (Balsa- Canto et al., 2001). The problem is formulated as follows: Maximise the cost functional

(8.17) 퐽 = 푥푡푥푡 − 휌 [푢̇ (푡) + 푢̇ (푡)] 푑푡 subject to the dynamics given by

(8.18a) 푥̇ = 푢 + 푢,

(푢 + 푢 ) (8.18b) 푥̇ = 푔푥 − 푥, 푥

푢 (푢 + 푢 ) 푥 (8.18c) 푥̇ = 100 − 푥 − 푔 , 푥 푥 0.51

( ) 푢 + 푢 (8.18d) 푥̇ = 푅푥 − 푥, 푥

( ) 푢 푢 + 푢 (8.18e) 푥̇ = 4 − 푥, 푥 푥

(8.18f) 푥̇ = −푘푥,

푥̇ = 푘(1 − 푥), (8.18g) where the state variables are:

 푥, the reactor volume,

 푥, the cell density,

 푥, the nutrient concentration,

 푥, the foreign protein concentration,

 푥, the inducer concentration,

 푥, the inducer shock factor on cell growth rate, and

 푥, the inducer recovery factor on cell growth rate. 78

The control variables are 푢, the glucose rate, and 푢, the inducer feeding rate (Balsa-Canto et al., 2001). The terms 푔, 푘, 푘 and 푅 are given by

푥 0.22 푥 푔 = 푥 + , (8.19) 푥 0.22 + 푥 14.35 + 푥 + 111.5

0.09푥 푘 = 푘 = , (8.20) 0.034 + 푥

푥 0.0005 + 푥 푅 = 0.233 . (8.21) 푥 0.022 + 푥 14.35 + 푥 + 111.5

The objective is also subjected to the following bounds on the control variables

0 ≤ 푢, 푢 ≤ 1. (8.22)

For this problem, the final time is fixed at 푡 = 10 and the initial conditions on the state variables are 퐱(ퟎ) = [1,0.1,40,0,0,1,0].

The Lagrange term in the Bolza form objective function is introduced as a penalty on the rate of change of the two controls. The term is multiplied by 휌 = 0.1/푁 where 푁 is chosen to be the total number of discretisation nodes, forcing the penalty to approach 0 as 푁 gets large. 풫풪풫풯 does not allow for a control time derivative to be formulated explicitly. However, the problem can be reformulated by treating the two controls, 푢 and 푢, as state variables, 푥 and

푥, respectively. The control variables passed into 풫풪풫풯 are then pseudo-controls, 퓊 and

퓊, such that

퓊 = 푢̇ , 퓊 = 푢̇ , (8.23) and these are then substituted into the cost equation. Lastly, the dynamics of Eq. 8.18 are augmented to include the following

푥̇ = 퓊, 푥̇ = 퓊. (8.24)

This problem has been solved by 풫풪풫풯 and compared with solutions obtained through the open-source C++ package 풫풮풪풫풯 and the commercial MATLAB package PROPT. As a license for PROPT was unable to be obtained, the comparison has been made with the solution provided in the PROPT user’s guide (Rutquist and Edvall, 2010). Furthermore, as 풫풮풪풫풯 cannot yet be compiled on Windows 10, the comparison against it has also been made using the solution provided in its user’s guide (Becerra, 2009).

풫풪풫풯 was initialised with the same initial grid used within the 풫풮풪풫풯 solution. This initial grid contained three intervals with the first interval having 20 nodes, the second having 35 nodes and the third having 50 nodes. The discretisation error tolerance required to be satisfied was set to 휖 =1×10. As PROPT does not have an in-built grid refinement algorithm, a manual technique was used by Rutquist and Edvall (2010). In this technique, the problem was first solved on a course grid and this solution was then used as the initial guess on a finer grid. In total, four iterations were executed with the number of nodes in each iteration being 20, 35, 55 and 85, respectively. Fortuitously, the same initial guess was used by both 풫풮풪풫풯 and PROPT, allowing the same settings to be provided to 풫풪풫풯. A summary of the performance of each solver is provided in Table 8.5. The CPU time shown for 풫풪풫풯 is the average of 15 runs. The code used to solve this problem in 풫풪풫풯 is provided in Appendix E.

Table 8.5: Comparison of solution procedure from 퓟퓞퓟퓣, 퓟퓢퓞퓟퓣 and PROPT for the Lee-Ramirez Bioreactor problem. Maximum Number of Grid IPOPT CPU Time Computed Solver Discretisation Iterations (sec) Objective Value Error

풫풪풫풯 1.7967 × 10 1 7.057 6.1510897

풫풮풪풫풯 1.8967 × 10 1 19.43* 6.0493740

N/A PROPT N/A 209.15∗ 6.1494162 (solved 4 times)

From the performance summary in Table 8.5, it can be seen that 풫풪풫풯 is able to outperform both 풫풮풪풫풯 and PROPT in terms of the optimality of the objective and the discretisation error. Furthermore, despite the fact that the error tolerance was set to 1×10, 풫풪풫풯 was able to yield a discretisation error of 1.7967 × 10 on the first iteration. In comparison, the error from 풫풮풪풫풯 was an order of 10 larger. The superior performance of 풫풪풫풯 can in part be

* These computation times are only shown for completeness and not for comparative purposes. These times were the values reported in the respective user’s guides for the two packages. 80

attributed to the chosen collocation methods for each of the solvers. PROPT uses the LG method of collocation and 풫풮풪풫풯 uses the LGL method. As outlined in Section 2.4.7 and 3.1.2, the LGR collocation method is superior to both the LG and LGL methods in terms of solution accuracy and optimality. The state and control solutions obtained by 풫풪풫풯 are provided in Figs. 8.8 and 8.9, respectively.

Figure 8.8: State solution for Lee-Ramirez Bioreactor problem obtained by 풫풪풫풯.

Figure 8.9: Control solution for Lee-Ramirez Bioreactor problem obtained by 풫풪풫풯.

Recall that the aim of this problem was to maximise the objective function. As can be seen in Table 8.5, the value of the objective computed by 풫풮풪풫풯 is lower than the values found by both 풫풪풫풯 and PROPT. Upon closer examination of the control trajectories, it was seen that 풫풮풪풫풯’s solution differs significantly from the solutions obtained by both 풫풪풫풯 and PROPT (which were in agreeance). The control trajectories from 풫풮풪풫풯 are shown in Fig. 8.10 and PROPT’s trajectories can be found in Rutquist and Edvall (2010). As can be seen, 풫풮풪풫풯’s controls reduce in magnitude within the 9 to 10 second time interval, whereas the control in the 풫풪풫풯 solution stays constant. Furthermore, the magnitudes of the controls in this time interval are also vastly different, with 풫풪풫풯’s controls being around 0.90 to 1 and 풫풮풪풫풯’s controls ranging from 0.55 to 0.60. As the objective from 풫풪풫풯 is higher than what 풫풮풪풫풯 computed, and both 풫풪풫풯 and PROPT are in agreeance, it can be concluded that the solution computed by 풫풮풪풫풯 is in fact sub-optimal.

Figure 8.10: Control solution for Lee-Ramirez Bioreactor problem obtained by 풫풮풪풫풯 (Becerra, 2009). The solution during the time 9 to 10 second time interval is seen to be considerably different to the solution found by both 풫풪풫풯 and PROPT.

8.6 Example 5 – Goddard Rocket

Consider the following classical optimal control problem known as the Goddard Rocket problem (Bryson and Ho, 1975). The aim of this problem is to find the optimal thrust trajectory such that the final altitude is maximised. This is formulated as follows: Maximise the Mayer form cost functional

퐽 = ℎ푡 (8.25) subject to the dynamic constraints,

ℎ̇(푡) = 푣(푡), (8.26a)

푇(푡) − 퐷(푣, ℎ) 푣̇(푡) = − 푔, (8.26b) 푚(푡)

푇(푡) 푚̇ (푡) = − , (8.26c) 푉 the constant bounds,

0 ≤ 푇(푡) ≤ 푇 = 193, (8.26d) and the boundary constraints,

ℎ(0) = 푣(0) = 0, 푚(0) = 3, 푚푡 = 1. (8.26e)

Here ℎ(푡) is altitude, 푣(푡) is velocity, 푚(푡) is mass, 푇(푡) is thrust (and the control input),

퐷(푣,ℎ) is drag force, 푔 is gravitational acceleration, and 푉 is the exhaust velocity (1580.9425 ft/s). The drag force, which is a function of the altitude and the velocity, is modelled as

퐷(푣, ℎ) = 퐷푣 exp (−ℎ/퐻), (8.27)

where 퐷 = 5.49153485 × 10 and 퐻 is the density scale height equal to 23800. Using Eqs. 8.25 – 8.27 this example is formulated as a single-phase problem. However, the Goddard Rocket problem is well known to contain a singular arc throughout one segment of its optimal thrust profile. A singular arc occurs when the control cannot be uniquely approximated from

the optimality conditions because the first and second order partial derivatives of the Hamiltonian, with respect to the control, are both zero (Betts, 2010). This situation commonly occurs in the absence of path constraints and when the control appears linearly in the equations of motion and the Hamiltonian, as it does for the Goddard Rocket problem (Betts, 2010). In such a situation, the NLP resulting from the transcription process will be singular and an NLP solver will not be able to determine the control accurately. The Goddard Rocket problem experiences a singular arc whenever the thrust profile lies on the interior of the bounds defined in Eq. 8.26d, as this leads to specific conditions being satisfied that cause the first and second order control partial derivatives of the Hamiltonian to be singular (Limebeer and Rao, 2015). To demonstrate this, the problem was passed to 풫풪풫풯 using a single-phase formulation. The computed control history is shown in Fig. 8.11. The solution shows oscillatory behaviour, which is commensurate with the singular arc. This is because the control is not able to be uniquely determined.

Figure 8.11: Control solution for a single-phase formulation of the Goddard Rocket problem.

To obtain an accurate solution, the necessary condition for a singular arc to occur can be imposed as a constraint, thus removing the singularity and providing an equation to uniquely determine the control (Betts, 2010; Patterson and Rao, 2012b). This condition is

푑 (ℋ ) = 0, (8.28) 푑푡 where ℋ is the Hamiltonian. To enforce this condition, the Goddard Rocket problem is required to be formulated as a three-phase problem. This highlights the importance and necessity of multi-phase solvers. 84

Within all three phases, the constraints of Eq. 8.26 must be satisfied. As per Betts (2010), to impose the condition of Eq. 8.28, the following path constraint must hold in phase 2

푉 푣 2푉 1 + − 1 + 퐻푔 푉 푣 푇(푡) − 퐷(푣,ℎ)− 푚푔 1+ = 0, (8.29) 4푉 푉 1 + 푣 + 2 푣 along with the following boundary condition to signal the end of the singular arc

푚푔 − (1 + 푣/푉) 퐷(푣, ℎ) = 0. (8.30)

Furthermore, the following phase linkage constraints must be imposed on the states and time

() () 퐱 푡 = 퐱 푡 , (8.31a)

() () (8.31b) 푡 = 푡 , where 푝 =1, 2 and the notation ( . )() is used to represent terms in the 푝-th phase. The objective function for the three-phase problem is then modified to be

() 퐽 = ℎ푡 . (8.32)

The problem has been solved with 풫풪풫풯 and 픾ℙ핆ℙ핊 − 핀핀. Both solvers were initialised with the same initial guess and 24 LGR points across two grid intervals in all phases of the problem (i.e. 6 intervals and 72 LGR points in total). The maximum required discretisation error was set to 휖 = 10. The derivative calculation method in 풫풪풫풯 was set to automatic- differentiation. Similarly, derivatives were calculated in 픾ℙ핆ℙ핊 − 핀핀 using it’s automatic- differentiation tool ADiGator (Weinstein and Rao, 2015). A summary of the performance is provided in Table 8.6, and Fig. 8.12 shows the state and control histories. The code to solve this problem using 풫풪풫풯 is provided in Appendix F.

Table 8.6: Comparison of solution from 풫풪풫풯 and 픾ℙ핆ℙ핊 − 핀핀 for the Goddard Rocket problem. Maximum IPOPT CPU Time Number of Grid Optimal Objective Solver Discretisation (sec) Iterations Value Error (15 run averages)

풫풪풫풯 7.9725 × 10 1 0.416 18550.87188

픾ℙ핆ℙ핊 − 핀핀 1.1504 × 10 1 0.605 18550.87186

Figure 8.12: State and control solution for the three-phase formulation of the Goddard Rocket problem.

Comparing the control of the multi-phase approach with the single-phase solution, several similarities can be seen. In particular, the control in phase 1 and 3 matches the control at the corresponding times of the single-phase solution. However, it can clearly be seen that the control solution in Fig. 8.12 has been resolved correctly by enforcing the conditions of Eq. 8.29 and Eq. 8.30.

Table 8.6 shows that both 풫풪풫풯 and 픾ℙ핆ℙ핊 − 핀핀 obtained identical objective values, had a similar discretistaion error and both required just one grid-iteration, with 풫풪풫풯 being the more computationally efficient of the two. Since both solvers managed to satisfy the error tolerance using just the initial grid, a direct comparison can be made between the two solutions at each LGR point. Across the three phases, the maximum difference between the state solutions obtained by 풫풪풫풯 and the state solutions obtained by 픾ℙ핆ℙ핊 − 핀핀 was 6.511 × 10. Similarly, the maximum difference for the control was 9.947 × 10 . This further validates the accuracy of 풫풪풫풯.

8.7 Chapter Summary

This chapter has demonstrated the types of problems solvable using 풫풪풫풯, and has benchmarked its performance against other open-source and commercial packages. From the five examples considered, 풫풪풫풯’s performance in terms of computation time and solution accuracy was seen to be on par with, or better than, the software it was tested against. As 풫풪풫풯 is to be an open-source code, it is pertinent to compare it with other open-source codes. The two tested within this chapter were OptimTraj and 풫풮풪풫풯. The one problem attempted using OptimTraj was not successfully solved, as it implements low order transcription methods and does not have a grid refinement algorithm. Along a similar line, 풫풮풪풫풯 produced a sub- optimal solution compared to both 풫풪풫풯 and PROPT for the Lee-Ramirez Bioreactor problem. Based off this analysis, it can be argued that 풫풪풫풯 is a superior alternative to these other open-source software.

The main package compared with 풫풪풫풯 was 픾ℙ핆ℙ핊 − 핀핀. This is because the two packages share many similarities, such as the choice of collocation method, the ability to compute first and second order derivatives, they both have an in-built grid refinement algorithm, and they both use IPOPT as their NLP solver (although 픾ℙ핆ℙ핊 − 핀핀 can also be interfaced with SNOPT). Being a commercial package, 픾ℙ핆ℙ핊 − 핀핀 has many additional features to 풫풪풫풯: the ability to solve problems with static parameters, six different techniques to scale a problem, and four different grid refinement algorithms. Nevertheless, the comparisons carried out within this chapter show that 풫풪풫풯 is a competitive, open-source alternative to 픾ℙ핆ℙ핊 − 핀핀.

This chapter has also shown that 풫풪풫풯 can solve infinite-horizon problems as well as problems with rate constraints and singular arcs. Infinite-horizon problems in particular are a type of problem that are not able to be solved by other pseudospectral software such as PROPT and 풫풮풪풫풯 due to their choice of collocation method.

Chapter 9 Haul Truck Energy Management Case Study (HTEMCS)

This chapter solves the HTEMCS that was first presented in Chapter 1. The idea of this problem is to select the optimal trajectories for the usage of the engine and stored power, such that the fuel consumption and cycle time for a haul truck are minimised. The truck’s haul route and all of its characteristics, such as road gradient and speed limits, are known in advance.

This chapter first presents a review of relevant literature regarding methods of solving similar problems to the case study. The problem is then formulated and solved using 풫풪풫풯. The outcome of this case study will be insights into the optimal usage of the energy that is recovered and stored by an ESS.

9.1 Review of Optimal Energy Management Strategies

In open literature several contributions can be found regarding the optimisation of an energy management strategy (EMS) for conventional vehicles and for hybrid electric vehicles (HEVs). These optimisation strategies implement a variety of optimal control techniques such as dynamic programming (DP) as well as indirect and direct transcription methods.

In Stoicescu (1995), optimality conditions for conventional vehicles were derived using PMP to obtain optimal velocity trajectories subject to time constraints. It was found that under constant load conditions the optimal velocity trajectory was a period of full power acceleration, followed by a period of constant velocity, a period of coasting and lastly a period of hard deceleration. In Van Keulen et al. (2010a) it was remarked that optimal velocity trajectories for conventional vehicles are not necessarily optimal for HEVs due to the latter’s energy recovering potential. It was also shown for HEVs that deceleration periods contribute significantly to the solution optimality because they determine the amount of energy stored. When route information is known at a priori, as is the case for the HTEMCS, some periods of deceleration can be predicted, and this information can be exploited to obtain better solutions.

In Van Keulen et al. (2010b), it was found that by solving a velocity trajectory optimisation to minimise fuel consumption for HEVs, a total power trajectory is also obtained. It was also 88

shown that this total power trajectory could then be used to optimise the power-split trajectories between the engine and energy storage systems. The problem of finding the optimal power- split trajectory was shown to be a non-linear, non-convex constrained optimisation problem. DP was used at a slow update rate to calculate the optimal power-split trajectories offline. The extension to this DP approach is to then use the optimised solution in an online implementable energy management system. Van Keulen et al. (2010b) employed a low-level feedback controller approach with adaptive cruise control, whereas Hellstrom et al. (2010) implemented Model Predictive Control (Wang, 2009). The implementation of a real-time strategy is beyond the scope of this thesis. DP techniques have also been used by Kirschbaum et al. (2002), Lin et al. (2002), Koot et al. (2005) and Mensing et al. (2012).

In all the above papers using DP, the control variables were chosen to be the power of the engine and storage system. The states were chosen to be the state of energy (SOE) of the storage system and, in most cases, the vehicle position and velocity. These are the only state variables required to solve the HTEMCS (Sciarretta and Guzzella, 2007).

Whilst the DP algorithm has been used extensively, it performs best under simple route regimes, such as routes with a single speed limit or constant gradient (Albrecht et al., 2016). Taking instantaneous changes in route characteristics into consideration has proved to be challenging for a global optimisation algorithm such as DP (Wang, 2017).

Paganelli et al. (2000) devised a static-optimisation based strategy to solve the power-split problem. Their approach is called the Equivalent Consumption Minimisation Strategy (ECMS) and provides a real-time implementable algorithm to minimise fuel-consumption of HEVs. The algorithm produces sub-optimal results. A survey of this method and example implementations are provided in Sciarretta and Guzzella (2007). To summarise, the method converts the stored energy to an equivalent fuel power through equivalence factors. These factors represent an averaged chain of efficiencies that transform the fuel to electrical power and vice-versa (Onori et al., 2010). The optimality of this method is extremely sensitive to the equivalence factors and requires careful tuning. Methods of tuning have been proposed using penalty methods (Paganelli et al., 2000), feedback controllers for the current SOE (Koot et al., 2005) or automatic tuning (adaptive) strategies (Onori et al., 2010).

For a vehicle travelling on a route with changing speed limits and road gradients, the control strategy can also be expressed as a multiple-phase trajectory optimisation problem. The way to pose this is to force the differential algebraic equations (DAEs) to remain unchanged within a

single phase. In other words, different sets of DAEs must be formulated in different phases. This allows for discontinuities in route characteristics to be handled in an intuitive manner, overcoming an issue that has been observed when using DP. Wang (2017) has implemented this approach to minimise fuel consumption of trains using the tractive and braking force as controls. The problem was solved using 픾ℙ핆ℙ핊 − 핀핀 and a total of 125 phases were used for a 50 km route. The computation time was 46 seconds to complete the optimisation. When compared to methods such as DP, this is a considerably lower computation time. 픾ℙ핆ℙ핊 − 핀핀 has also been successfully used by Limebeer et al. (2014) to optimise the usage of an energy recovery system installed upon a Formula One vehicle. The performance index was to minimise fuel usage and lap-times. This is a similar problem to the HTEMCS in that it optimises the use of an energy recovery system, but differs in the respects that no speed limit constraint was applied and the vehicle was not required to decelerate to a stop at the end of a lap. This meant that the problem was able to be solved using a single-phase approach. Nevertheless, the fact that trajectory optimisation methods have been used to successfully solve problems with significant relevance to the case study, provides evidence that this approach can be feasibly applied to solve the HTEMCS.

Based on a review of surrounding literature, it can be seen that the HTEMCS is approachable in many ways. Whilst several methods are viable, multiple-phase trajectory optimisation is viewed as an appropriate choice, as it has potential to keep computation times low whilst still achieving near-optimal solutions. This method also provides a platform to deal with discontinuities in route characteristics, objective functions, dynamics and path constraints; a task which other methods such as DP have proven to struggle with.

9.2 General Problem Setup for the HTEMCS

The HTEMCS has been based on the work done in Terblanche et al. (2018). The energy recovery system that has been selected is an electromechanical flywheel with a rated power of

1390 kW, an energy storage capacity of 퐸 = 46.3 kWh and a mass of 4574 kg. The truck selected for the problem is the Caterpillar 795F AC (Caterpillar, 2013). The configuration for a drive system with the energy recovery system installed, as proposed by Terblanche et al. (2018), is illustrated in Fig. 9.1.

Figure 9.1: Drive system configuration for a DEMHT with the energy recovery system installed (Terblanche et al. 2018).

The mathematical model of the vehicle dynamics is given by the following equation (Guzzella and Sciaretta, 2013)

푑 푚 푣(푡) = 퐹 (푡) − 퐹 (푡) + 퐹 (푡) + 퐹 (푡) + 퐹 (푡), (9.1) 푑푡

where 푚 is the lumped mass of the vehicle, payload and the energy recovery system, 푣(푡) is the vehicle’s velocity, 퐹(푡) is the tractive force, 퐹(푡) is the rolling resistance, 퐹(푡) is the gravitational force, 퐹(푡) is the aerodynamic force and 퐹(푡) is the force of the brakes. Note that for any given time 푡 we must have 퐹(푡) ⋅퐹(푡) =0 as the traction and brake forces cannot act simultaneously. Furthermore, 퐹(푡) =퐹(푡) +퐹(푡), indicating that the tractive force is supplied by both the engine and the electric motor. The operational limits of the rimpull and the dynamic braking force available at various vehicle velocities are provided in Fig 9.2. This information has been extracted from Caterpillar (2013). At low velocities the rim-pull is torque-limited, whilst at high velocities it is power-limited.

Figure 9.2: Rim-pull and dynamic brake force available at various velocities (Caterpillar, 2013).

The resistive forces, 퐹(푡), 퐹(푡) and 퐹(푡) have been modelled as per Terblanche et al. (2018).

퐹(푡) = 0.02푚푔 (9.2a)

퐹(푡) = 퐺푚푔 (9.2b)

퐹(푡) = 0.5퐶휌퐴푣 (9.2c)

where 푔 is acceleration due to gravity, 퐺 is the percentage road grade, 퐶 is drag coefficient and equal to 0.9, the density of air is given by 휌 = 1.2 kg/m and the cross-sectional area of the truck is approximated as 퐴 = 70 m.

To formulate the case study as a trajectory optimisation problem, the characteristics of the haul route must first be defined. The selected haul route is outlined in Table 9.1.

Table 9.1: Haul route definition. Sector Sector length Road grade Speed limit Truck mass Load Number (m) (%) (km/h) (1000 kg)

1 50 0 15 250

2 503 −10 30 250 Unladen

3 100 0 30 250

4 503 10 30 570 Fully loaded 5 50 0 15 570

Each sector of the problem either has a discontinuous change in road gradient, speed limit or truck mass. Hence, the case study is required to be set up as a five-phase problem where each phase corresponds to a sector as defined in Table 9.1.

9.3 Multi-Phase Trajectory Optimisation Formulation for the HTEMCS

The problem is now formally set up as a multi-phase trajectory optimisation problem. The notation (⋅)() is used to specify information in the 푝th phase, for 푝 = {1,…,5}. Over each phase of the problem, the following Bolza form cost functional is to be minimised,

( ) () () () (9.3) 퐽 = 푡 − 푡 + 휂푢 (푡) 푑푡 , () subject to the dynamic constraints,

() () 푢 (푡)/휂 + 휂푢 (푡) () (9.4a) 푥̇ (푡) = − , 퐸

() () (9.4b) 푥̇ (푡) = 푥 (푡),

() () () () 퐹 (푡) − 퐹 (푡) + 퐹 (푡) + 퐹 (푡) () (9.4c) 푥̇ (푡) = , 푚

풫()(푡) ()( ) (9.4d) 퐹 푡 = () , 푥

() () () () 풫 (푡) = 훼휂푢 (푡) + 훽휂푢 (푡) + 훾휂푢 (푡), (9.4e)

() () () () where 푥 (푡) is the SOE, 푥 (푡) is distance travelled, 푥 (푡) is velocity, 퐹 (푡) is tractive () () force and 풫 (푡) is the total power output. The controls are the power from the engine, 푢 (푡), () () the power from the ESS, 푢 (푡), and the power of the braking system, 푢 (푡). The SOE is formally defined as

() () () E − ∫ () 푢 (푡)/휂 + 휂푢 (푡) 푑푡 () (9.4f) 푥 (푡) = . 퐸

where 퐸 is the maximum energy capacity of the storage system (166.7 MJ).

Note that 훼, 훽 and 훾 are all binary values, i.e. 훼, 훽, 훾 ∈ {0,1}, with 훼 ⋅ 훾 =0 and 훽 ⋅ 훾 =0 to ensure the that brake and propulsive forces do not act simultaneously. The system efficiencies are given as

휂 = 휂 ⋅ 휂 = 0.95 ⋅ 0.85 = 0.8075,

휂 = 휂 ⋅ 휂 = 0.90 ⋅ 0.85 = 0.7650, (9.5)

휂 = 휂 ⋅ 휂 = 0.90 ⋅ 0.85 = 0.7650,

where 휂 is the efficiency of the diesel generating unit (engine to DC link), 휂 is the drive efficiency (DC link to wheels and vice versa) and 휂 is the energy storage efficiency (DC link to storage and vice versa) (Terblanche et al., 2018).

The cost of Eq. 9.3 is also subjected to the following path constraints

() () 0 ≤ 푥 (푡) ≤ 푉(푡),

() 0 ≤ 푢 (푡) ≤ 푃(푡),

() 0 ≤ 푢 (푡) ≤ 푃 (푡), (9.6) ( ) ()( ) 푃 푡 ≤ 푢 푡 ≤ 0,

() () 0 ≤ 푢 (푡) + 푢 (푡) ≤ 푃(푡)

() 퐴 ≤ 푥̇ (푡) ≤ 퐴.

The first constraint ensures that the velocity of the truck never exceeds the speed limit. The next three constraints are the maximum and minimum allowable power outputs for the engine, storage system and brake, respectively. The maximum power of the engine is determined using the rim-pull curve of Fig. 9.2. This enforces the rim-pull constraint. The power of the storage system delivered to the wheels is controlled by in-wheel electric motors and is limited to 1390 kW as per the design of the electromechanical flywheel by Terblanche et al. (2018). The brake power limits are also determined from Fig. 9.2. The fifth constraint is enforced to ensure that the sum of the engine and electric power sources are constrained to be within the limits defined by the rim-pull curve of Fig. 9.2. The sixth, and final, constraint limits the acceleration

of the truck to remain within comfortable bounds for a human driver. This has been sourced to be approximately 0.6 m/s (Hoberock, 1977).

Referring back to Table 9.1, the phases of the problem are essentially dependent on the position of the truck within the haul route. Hence, the end of one phase and the start of the next is simply enforced using the distance travelled by the truck.

9.4 Results of the HTEMCS

The problem was passed to 풫풪풫풯 with the following initial grid

 Phase 1: 1 interval with 8 LGR points,  Phase 2: 15 intervals with 4 LGR points each,  Phase 3: 1 interval with 8 LGR points,  Phase 4: 10 intervals with 8 LGR points each, and  Phase 5: 10 intervals with 8 LGR points each.

The maximum discretisation error was set to 휖 = 10 and the problem was solved in first derivative mode using complex-step differentiation. The problem was initialised with straight line initial guesses. For example, the distance travelled was guessed with a straight line between the start and end distance for each phase, and the velocity was guessed to be constant at the speed limit within each phase. Similarly, simple guesses were posed for the remaining decision variables, being the SOE, the controls as well as the initial and terminal time for each phase. The problem was scaled using the in-built automatic scaling routine, which scales the variables to specified constant bounds. A summary of the solution procedure is provided in Table 9.2. Figures 9.3 and 9.4 show the state and control histories, respectively. The code to solve this problem using 풫풪풫풯 is provided in Appendix G.

Table 9.2: Summary of solution procedure for HTEMCS. Number IPOPT CPU Time Maximum Number of IPOPT Optimal Objective of Grid (sec) Discretisation Error NLP iterations Value Iterations (15 run averages)

Phase 1 = 1.67 × 10 Phase 2 = 1.61 × 10 Mayer: 296.5 Phase 3 = 5.17 × 10 10 8371 47.744 Lagrange: 2.86 × 10 Phase 4 = 7.62 × 10 Phase 5 = 3.23 × 10

Figure 9.3: State solution for HTEMCS – Min Time and Min Fuel Formulation.

Figure 9.4: Control solution for HTEMCS – Min Time and Min Fuel Formulation.

The results of the HTEMCS paint an interesting picture regarding the usage of the ESS. Within phase one, the vehicle uses enough power to reach the speed limit before it reduces the power usage to maintain velocity. In the descent phase, the solver allows the vehicle to gain velocity simply due to gravitational acceleration. This is the optimal approach to ensure minimum fuel is expended. Once the speed limit is reached, the brakes are applied to maintain the velocity and satisfy the speed limit constraint. As the vehicle brakes, the SOE increases. At the end of this phase the vehicle undergoes a period of hard deceleration as the road gradient changes. The more interesting behaviour occurs from phase three onwards. In phase three, the vehicle is travelling on flat ground with a speed limit of 30 km/h, but the solver chooses to

allow the vehicle to coast and lose velocity in order to minimise fuel consumption. This is despite the fact that this phase has a minimum time objective. The reason for this is the constraint on the maximum allowable acceleration or deceleration. If the vehicle had maintained velocity at the speed limit, a much larger rate of deceleration would have been required as the vehicle got near phase four. The solver also does not expend any of the stored power within phase three, instead choosing to keep it stored for later use when it will be of greater benefit. Within phase four, the maximum speed the vehicle can reach with the additional 320 t load and the inclined gradient is approximately 10 km/h. The solver chooses to use the stored energy within this phase after foregoing its usage in the previous stage. The rate at which the stored energy is reinjected is chosen such that the stored energy can be used for the entirety of the incline phase. The release rate is essentially constant at some averaged amount, with the oscillatory behaviour seen in Fig. 9.4 being an artefact of the solution. The final phase of the trajectory again has the vehicle travelling on flat ground with the condition that the vehicle come to a stop at the end of the route. The optimal trajectory for this phase is seen to mirror the optimal trajectory computed by Stoicescu (1995) using PMP for a HEV (see Section 9.1). That is, the optimal trajectory has a period of full power acceleration to reach the speed limit, followed by a small period of constant velocity, a period of coasting and lastly a period of hard deceleration to bring the truck to a stop. This adds validation to the solution computed by 풫풪풫풯.

As an additional analysis, a problem with only a minimum time objective was also solved. The results are shown in Figs. 9.5 and 9.6. For this formulation, the total cycle time computed for the haul route was 284.5 seconds, compared to 296.5 seconds for the previous formulation. As can be seen, the power supply from the engine is relied upon considerably more, particularly within the ascent phase. An interesting aspect of the solution in the fourth phase is that the solver still chose to use the storage power to replace a small portion of the engine power (the ratio of engine to stored power being used is approximately 11:1). However, this had no impact on the cycle time because the same value was achieved even if a constraint was imposed to forbid the use of the stored energy. The velocity of the vehicle was again only able to reach around 10 km/h due to the rim-pull constraint, the impact of the incline and the payload. The reduction in the cycle time compared to the minimum time/minimum fuel formulation occurs mainly in phase five. Within the minimum time formulation, a period of coasting is no longer present in phase five because the fuel consumption objective is not present.

Figure 9.5: State solution for HTEMCS – Min Time Formulation.

Figure 9.6: Control solution for HTEMCS – Min Time Formulation.

Chapter 10 Limitations of the Software

As with all software, 풫풪풫풯 is not without its limitations. Known limitations and potential issues include the following:

 Dynamics and constraint functions must have continuous first and second derivatives within a phase.  Solutions obtained by 풫풪풫풯 are only locally optimal as IPOPT is only a local solver.  The performance of the in-built automatic scaling routine is dependent upon accurate bounds being provided by the user. If the bounds are inaccurate, the scaling routine can adversely affect the solution.  Phases in multiple-phase problems must be connected sequentially.  The number of states within a multiple-phase problem must remain constant across phases.  Static parameter optimisation is not supported.  There is no option for the user to provide analytical derivatives.  The automatic differentiation toolbox used within 풫풪풫풯 does not support MATLAB functions that use the repmat() function.  The following MATLAB functions can lead to issues when using complex-step differentiation: abs(), min(), max() and dot().  Care must be taken when using complex-step differentiation to ensure that transposes are taken using the “dot-transpose” method in MATLAB and not the “complex- conjugate transpose” (Martins et al., 2003).

100

Chapter 11 Conclusions and Recommendations for Future Work

11.1 Conclusions and Summary of Outcomes

The primary aim of the thesis was to develop a software to solve multiple-phase trajectory planning problems, as a scope was identified to fill voids left by current open-source packages. A MATLAB implementation called the Pseudospectral OPTimiser (풫풪풫풯) has been developed that can solve such problems using highly accurate, variable-order numerical integration schemes. From the literature review, it was found that globally orthogonal collocation methods, also known as pseudospectral methods, were the most accurate and efficient techniques to numerically solve trajectory planning problems. These techniques fall under the umbrella of direct transcription, as they transcribe the continuous time trajectory planning problem into a sparse nonlinear program. 풫풪풫풯 was developed by employing a variable-order Legendre-Gauss-Radau (LGR) orthogonal collocation technique. This technique approximates solution trajectories using high-order Lagrange polynomials, then satisfies the system dynamics and constraints at the well-defined LGR quadrature points. A grid refinement method has been implemented that can determine both the degree of the approximating Lagrange polynomial, as well as the number of grid intervals required to achieve a desired solution accuracy. The nonlinear program that results from the transcription of the trajectory planning problem is solved using the interior-point optimisation algorithm IPOPT.

The second aim for the thesis was to test and validate the software on example problems from literature, and then assess its performance against other packages. The utility of 풫풪풫풯 has been tested and validated on five benchmark problems. The first of these was an infinite- horizon Linear Quadratic Regulator problem for which an exact solution was derived. 풫풪풫풯 was demonstrated to achieve an accurate solution, with the magnitude of error ranging from 10 to 10. In the other four problems, 풫풪풫풯 was compared against open-source and commercial packages. 풫풪풫풯’s performance was benchmarked to be better than all open- source packages that were compared in terms of solution accuracy and efficiency. The primary reasons for the superior performance were the choice of collocation method and the execution

101

of the grid refinement algorithm, which is something that many other open-source packages do not have. 풫풪풫풯 was also seen to have comparable performance with the commercial pseudospectral solver 픾ℙ핆ℙ핊 − 핀핀. Based off the outcomes of this thesis, 풫풪풫풯 has demonstrated an ability to solve a broad range of problems. With its strong performance and robustness, it can be concluded that the solver will be a valuable addition to the open-source community of optimal control solvers.

The third aim of the thesis was to optimise the solver for computational efficiency. Significant progress has been achieved, with a technique implemented that exploits the well- defined sparsity patterns of the constraint Jacobian and Lagrangian Hessian resulting from the Radau pseudospectral method. This is one of the main reasons behind the strong performance of 풫풪풫풯 in regard to computation times. Recommended future work to further improve the solver’s efficiency has been provided in the following section.

Lastly, a five-phase case study problem involving the use of energy recovery systems on- board Diesel electric mining haul trucks has been investigated and solved using 풫풪풫풯. The results highlighted that the optimal time to reinject stored energy for propulsion purposes is under maximum load conditions, which was during the fully loaded incline phase. The rate of release is such that the stored energy is used for the duration of the incline, with all of the energy reinjected by the end. This study has provided insights into the optimal usage of these energy recovery systems.

The source code of the software, which is the main contribution of this thesis, has been made available on the hosting service GitHub (github.com/InderpreetMetla/popt). The hope is that the software will continue to be improved and built upon in the future.

102

11.2 Recommendations for Future Work

There are many opportunities to improve upon the developed software. Recommendations for future work regarding 풫풪풫풯 are listed in Table 11.1.

Table 11.1: Recommendations for future work for 풫풪풫풯. Task Remarks

A second NLP solver, such as SNOPT or LOQO, will provide a user Add a second NLP solver. greater flexibility in solving a problem. NLP solvers can perform better or worse than others, depending on the problem.

This task will require development of new sparsity patterns as static Include static parameter parameters will also appear in the constraint Jacobian matrices. This optimisation capabilities. is a desirable feature as problems with static parameters are quite common (Betts, 2010).

This task will require both the interface and the internal structure of the software to be significantly modified. Rather than constructing Modify the software so non- the phase linkage constraint functions internally, as 풫풪풫풯 currently sequential phase connection does, non-sequential phase connections will require the user to is permissible. provide a linkage constraint function. The consequence will be a more general type of problem solvable by the software.

Internal function semantics will need to be modified and new Allow user to provide exact functions will need to be created that allow for exact derivatives to derivatives. be provided by the user.

The only finite-difference technique currently implemented for both first and second derivatives is forward-difference. 풫풪풫풯 has been Implement central-difference designed in a modular way that permits additional derivative derivative approximations. approximation methods to be included in the code in a relatively straightforward manner.

A feature of MATLAB is that any function evaluated with a variable set to the input (“not-a-number”) will output if the Determine function NaN NaN dependencies for further function depends on said variable. This can be used to construct a computational efficiency. map outlining precisely which elements of a Jacobian or Hessian need to be evaluated. This will allow for the sparsity patterns to be exploited even further, leading to greater computational efficiency.

Rewrite software in a low- A low-level language, such as C++, offers significant performance level programming language. improvements over a high-level language such as MATLAB.

For the haul truck energy management case study, recommendations for future work are:

 Validate the results using dynamic programming because this technique is used extensively in the literature,  Include modelling of steering and more complex haul routes, and  Implement a real-time strategy, such as Model Predictive Control, which can be used online. 103

References

Agamawi, Y. M., & Rao, A. V. (2018). “Exploiting Sparsity in Direct Orthogonal Collocation Methods for Solving Multiple-Phase Optimal Control Problems”. In 2018 Space Flight Mechanics Meeting (p. 0724).

Albrecht, A., Howlett, P., Pudney, P., Vu, X. and Zhou, P. (2016). “The key principles of optimal train control. Part 1: formulation of the model, strategies of optimal type, evolutionary lines, location of optimal switching points”. Transportation research part B: methodological, Vol. 94, pp. 482-508.

Albuquerque, J., Gopal, V., Staus, G., Biegler, L. T., & Ydstie, B. E. (1999). “Interior point SQP strategies for large-scale, structured process optimization problems”. Computers & Chemical Engineering, 23(4-5), 543-554.

Andersson J. (2013). "A General-Purpose Software Framework for Dynamic Optimization”. PhD Thesis, Arenberg Doctoral School, KU Leuven, Belgium.

Balsa-Canto, E., Banga, J. R., Alonso, A. A., and Vassiliadis, V. S. (2001). “Dynamic optimization of chemical and biochemical processes using restricted second-order information” . Computers & Chemical Engineering, 25(4-6), pp. 539-546.

Baydin, A. G., Pearlmutter, B. A., Radul, A. A., & Siskind, J. M. (2017). “Automatic Differentiation in Machine Learning: A Survey”. Journal of machine learning research, 18(153), 1-153.

Becerra, V.M. (2009). "PSOPT Optimal Control Solver User Manual”. University of Reading School of Systems Engineering.

Bertsekas, D. P. (2005). “Dynamic programming and optimal control (Vol. 1, No. 3)”. Belmont, MA: Athena scientific.

Betts, J.T. (1998). "Survey of Numerical Methods for Trajectory Optimization”. Journal of Guidance, Control and Dynamics. Vol. 21, No. 2, pp. 193-207.

Betts, J.T. and Huffman, W. P. (1998). “Mesh Refinement in Direct Transcription Methods for Optimal Control”. Optimal Control Applications and Methods, 19:1-21. 104

Betts, J. T. and Huffman, W. P. (1999). “Exploiting Sparsity in the Direct Transcription Method for Optimal Control”. Computational Optimization and Applications, 14(2), pp. 179-201.

Betts, J.T. (2010). “Practical Methods for Optimal Control and Estimation Using Nonlinear Programming”. Society for Industrial and Applied Mathematics (SIAM), Second Edition.

Benson, D. A. (2004). “A Gauss Pseudospectral transcription for optimal control”. Ph.D. thesis, Department of Aeronautics and Astronautics, Massachusetts Institute of Technology, Cambridge, Massachusetts.

Benson, D. A., Huntington, G. T., Thorvaldsen, T. P. and Rao, A. V. (2006). “Direct trajectory optimization and costate estimation via an orthogonal collocation method”. Journal of Guidance, Control and Dynamics 29, 6, 1435–1440.

Biegler, L. T. (2010). “Nonlinear Programming: Concepts, Algorithms, and Applications to Chemical Processes”. SIAM, Vol. 10.

Biggs, M.C. (1975). “Constrained Minimization Using Recursive Quadratic Programming”. Towards Global Optimization, North-Holland, pp. 341–349.

Bischof, C. H., Bucker, H. M., Lang, B., Rasch, A., & Vehreschild, A. (2002). “Combining Source Transformation and Operator Overloading Techniques to Compute Derivatives for MATLAB Programs”. Proceedings Second IEEE International Workshop on Source Code Analysis and Manipulation, pp. 65-72.

Boggs, P. & Tolle, J. (1995). “Sequential Quadratic Programming”. Acta Numerica, 4, pp. 1- 51.

Boyd, S. and Vandenberghe, L. (2004). “Convex Optimization”. Cambridge University Press, Cambridge, UK.

Bryson, A. E. (1996). "Optimal control-1950 to 1985”. IEEE Control Systems. Vol. 16, No. 3, pp. 26-33.

Bryson, A., and Ho, Y. (1975). “Applied Optimal Control”, Hemisphere Publishing, New York.

105

Bryson, A.E., Denham, W.F. and Dreyfus, S.E. (1963). “Optimal Programming Problems with Inequality Constraints I: Necessary Conditions for Extremal Solutions’, AIAA Journal Vol 1, No. 11, pp. 2544-2550.

Caterpillar. (2013). “795F AC Mining Truck (Doc. No.: AEHQ6882-01)”. Caterpillar, USA.

Cizniar, M., Salhi, D., Fikar, M. and Latifi, M. A. (2005). "DYNOPT - Dynamic Optimisation Code for MATLAB”. 13th Annual Conference Proceedings of Technical Computing Prague.

Darby, C.L., Hager, W. and Rao, A.V. (2010). “An hp-Adaptive Pseudospectral Method for Solving Optimal Control Problems”. Optimal Control Applications and Methods 32:476-502. de La Gorce, M. (2016). “MatlabAutoDiff”, martinResearch, GitHub. URL https://github.com/martinResearch/MatlabAutoDiff.

Diehl, M. and Gros, S. (2017). “Manuscript of Optimal Control”, University of Freiburg.

Diehl, M., Bock H.G., Diedam H. and Wieber, P. (2005). “Fast Direct Multiple Shooting Algorithms for Optimal Robot Control”. Fast Motions in Biomechanics and Robotics 2005, Heidelberg, Germany.

Elnagar, G., Kazemi, M. and Razzaghi, M. (1995). “The Pseudospectral Legendre method for discretising optimal control problems”. IEEE Transactions on Automatic Control 40, 10, 1793–1796.

Esfahanian, E. (2014). “Hybrid electric haulage trucks for open pit mining”. The University of British Columbia, Vancouver.

Falugi, P., Kerrigan, E. and Van Wyk, E. (2010). “Imperial College London Optimal Control Software User Guide (ICLOCS)”. Imperial College London.

Fahroo, F., and Ross, I. M. (2008). “Pseudospectral Methods for Infinite-Horizon Nonlinear Optimal Control Problems”. Journal of Guidance, Control, and Dynamics, 31(4), pp. 927-936.

106

Gath, P. and Wells, K. (2001). “Trajectory Optimization Using a Combination of Direct Multiple Shooting and Collocation”. In AIAA Guidance, Navigation and Control Conference and Exhibit. American Institute of Aeronautics; Astronautics.

Gardi A., Sabatini, R. and Ramasamy, S. (2016). "Multi-objective optimisation of aircraft flight trajectories in the ATM and avionics context”. Progress in Aerospace Sciences. Vol. 83, pp. 1–36.

Garg, D., Patterson, M., Hager W., Rao A.V. and Rao, D. (2017). “An Overview of Three Pseudospectral Methods for The Numerical Solution of Optimal Control Problems”. HAL Id: hal-01615132.

Gill, P.E., Murray, W. and Wright, M.H. (1981). “Practical Optimization”. Academic Press, London.

Gill, P. E., Murray, W., & Saunders, M. A. (2005). “SNOPT: An SQP algorithm for large- scale constrained optimization”. SIAM review, 47(1), pp. 99-131.

Gong, Q., Fahroo, F. & Ross, I.M. (2008). “Spectral Algorithm for Pseudospectral Methods in Optimal Control”, Journal of Guidance, Control and Dynamics, Vol. 31, No. 3, pp. 460- 471.

Guzzella, L. and Sciaretta, A. (2013). “Vehicle Propulsion Systems: Introduction to Modelling and Optimization”. Springer, 3rd Edition.

Han, S.P. (1977). “A Globally Convergent Method for Nonlinear Programming”. Journal of Optimization Theory and Applications, Vol. 22, pp. 297.

Hanson, F. B. (2007). “Applied stochastic processes and control for Jump-diffusions: modeling, analysis, and computation”, Vol. 13, SIAM.

Hargraves, C. R. and. Paris, S. W. (1987). “Direct Trajectory Optimization Using Nonlinear Programming and Collocation.” AIAA Journal of Guidance, Control and Dynamics 10 (4):338–42.

Hellstrom, E., Aslund, J. and Nielsen, L. (2010). “Management of Kinetic and Electric Energy in Heavy Trucks”. SAE International Journal of Engines, 3, 1152-1163.

107

Hoberock, L. L. (1977). “A survey of longitudinal acceleration comfort studies in ground transportation vehicles”. Journal of Dynamic Systems, Measurement, and Control, 99(2), pp. 76-84.

Hoffmann, C., Kirches, C., Potschka, A., Sager, S. and Wirsching, L. (2011). "MUSCODII User Manual”. Interdisciplinary Center for Scientific Computing (IWR) University of Heidelberg, Germany.

Huntington, G. T. (2007). “Advancement and Analysis of a Gauss Pseudospectral Transcription for Optimal Control Problems”, PhD thesis, Massachusetts Institute of Technology.

Huntington, G. T., Benson, D. A. and Rao, A. V. (2007a). “A Comparison of Accuracy and Computational Efficiency of Three Pseudospectral Method”. AIAA Guidance, Navigation and Control Conference and Exhibit, Guidance, Navigation and Control and Co-located Conferences.

Huntington, G. T., Benson, D. A. and Rao, A. V. (2007b). “Optimal configuration of tetrahedral spacecraft formations”. The Journal of the Astronautical Sciences 55, 2, 141–169.

Huntington, G. T., Benson, D. A. and Rao, A. V. (2005). “Post-optimality Evaluation and Analysis of a Formation Flying Problem via a Gauss Pseudospectral Method”. Proceedings of the 2005 AAS/AIAA Astrodynamics Specialist Conference, AAS Paper 05-339.

Inagawa, T. and Sakaki, K. (2017). "OpenGoddard”. Interstellar Technologies Inc.

Jacobson, D.H. and Mayne, D.Q. (1970). “Differential Dynamic Programming”. Elsevier.

Kameswaran, S. and Biegler, L. T. (2008). “Convergence rates for direct transcription of optimal control problems using collocation at Radau points”. Computational Optimization and Applications 41, 1, 81–126.

Kawajir, Y., Laird, C. D., & Waechter, A. (2015). “Introduction to IPOPT: a tutorial for downloading, installing, and using IPOPT”. URL http://www. coin-or. org/Ipopt/documentation.

Kelly, M. (2015). “Transcription Methods for Trajectory Optimization A beginner’s tutorial”. arXiv:1707.00284. 108

Kelly, M. (2016a). “Trajectory Optimization Overview and Tutorial”. Cornell Scientific Computation and Numerics Seminar.

Kelly, M. (2016b). "OptimTraj User's Guide".

Kelly, M. (2017). “An Introduction to Trajectory Optimization: How to Do Your Own Direct Collocation”. Society for Industrial and Applied Mathematics (SIAM), Vol. 59, No. 4, pp. 849-904.

Kirk, D. E. (2012). “Optimal control theory: an introduction”. Courier Corporation, pp. 216- 217.

Kirschbaum, F., Back, M. and Hart, M. (2002). “Determination of the Fuel-Optimal Trajectory for a Vehicle Along a Known Route”. In Proc. of the 15th IFAC Triennial World Congress.

Koot, M., Kessels, J.T.B.A., De Jager, B., Heemels, W., Van den Bosch, P. and Steinbuch, M. (2005). “Energy Management Strategies for Vehicular Electric Power Systems”. IEEE Transactions on Vehicular Technology, 54, 771-782.

Lancaster, P., and Rodman, L. (1995). “Algebraic Riccati Equations”. Clarendon press.

Lee, J., and Ramirez, W. F. (1994). “Optimal fed‐batch control of induced foreign protein production by recombinant bacteria”. AIChE Journal, 40(5), pp. 899-907.

Leek, V. (2016). “An Optimal Control Toolbox for MATLAB Based on CasADi”, Master of Science Thesis in Electrical Engineering Department of Electrical Engineering, Linköping University.

Li, N., Lei, H., Shao, L., Liu, T., & Wang, B. (2017). “Trajectory Optimization Based on Multi-Interval Mesh Refinement Method”. Mathematical Problems in Engineering, Vol. 2017.

Liebherr. (2014). “Mining Truck T 284 (Doc. No.: LME 11481630-0.85-02.14_enGB)”. Liebherr, USA.

Limebeer, D. J., and Rao, A. V. (2015). “Faster, higher, and greener: vehicular optimal control”. IEEE Control Systems, 35(2), pp. 36-56.

109

Limebeer, D.J., Perantoni, G. and Rao, A.V. (2014). “Optimal Control of Formula One Car Energy Recovery Systems”. International Journal of Control, Vol. 87, Issue 10, pp. 2065-2080.

Lin, C.C., Peng, H., Grizzle, J.W. and Kang, J.M. (2002). “Power Management Strategy for a Parallel Hybrid Electric Truck”. IEEE Transactions on Control Systems Technology, 11, 839-849.

Martins, J., Sturdza, P. & Alonso, J. (2003). “The Complex-Step Derivative Approximation”. ACM Transactions on Mathematical Software, Vol. 29, No. 3, pp. 245-262.

Mayne, D.Q. (1966). “A second-order gradient method of optimizing nonlinear discrete time systems”. Int J Control, Vol. 3, pp. 85-95.

Mensing, F., Trigui, R. and Bideaux, E. (2012). “Vehicle Trajectory Optimization for Hybrid Vehicles Taking into Account Battery State-Of-Charge”. IEEE Vehicle Power and Propulsion Conference.

Mittelmann, H. (2018). "Benchmarks for Optimization Software". AMPL-NLP Benchmark, URL http://plato.asu.edu/ftp/ampl-nlp.html.

Moore, J. (2018). “opty: Software for trajectory optimization and parameter identification using direct collocation”. The Journal of Open Source Software, 3(21):300.

Nocedal, J. and Wright, S. (2006). “Numerical Optimization”. 2nd. ed., Ch. 18. Springer.

Onori, S., Serrao, L., Rizzoni, G. (2010). “Adaptive Equivalent Consumption Minimization Strategy for Hybrid Electric Vehicles”. Proceedings of the ASME 2010 Dynamic Systems and Control Conference.

Open Source Initiative. (2006). “The MIT license”.

Ozaki, N., Campagnola, S., Yam, C.H. and Funase, R. (2015). “Differential Dynamic Programming Approach for Robust-Optimal Low-Thrust Trajectory Design Considering Uncertainty”. 25th International Symposium on Space Flight Dynamics.

Paganelli, G., Guerra, T.M., Delprat, S., Santin, J.J., Delhom, M. and Combes, E., “Simulation and assessment of power control strategies for a parallel hybrid car”. Proc. IMechE, Part D: J. Auto. Eng., Vol. 214, No. 7, pp. 705–717.

110

Patterson, M. A, Hager, W.M. & Rao, A.V. (2014). “A ph Mesh Refinement Method for Optimal Control”, Optimal Control Applications and Methods, Vol. 36(4), pp. 398-421.

Patterson, M. A. and Rao, A. (2012a). “Exploiting Sparsity in Direct Collocation Pseudospectral Methods for Solving Optimal Control Problems”. Journal of Spacecraft and Rockets, 49(2), pp. 354-377.

Patterson, M. A. and Rao, A. (2012b). “GPOPS-II: A MATLAB Software for Solving Multiple-Phase Optimal Control Problems Using hp–Adaptive Gaussian Quadrature Collocation Methods and Sparse Nonlinear Programming”. ACM Trans. Math. Soft. 39, 3, Article 1 (July 2013).

Pan Y. and Theodorou, E. (2014). "Probabilistic Differential Dynamic Programming”. Advances in Neural Information Processing Systems 27.

Pontryagin, L. S. (1962). “The mathematical theory of optimal processes”. John Wiley.

Powell, M.J.D. (1978). “A Fast Algorithm for Nonlinearly Constrained Optimization Calculations”. Numerical Analysis, G.A.Watson ed., Lecture Notes in Mathematics, Springer Verlag, Vol. 630.

Quarteroni, A., Sacco, R. and Saleri, F. (2000). “Numerical Mathematics”. Vol. 37 of Texts in Applied Mathematics, Springer.

Rao, A.V. (2009). “A Survey of Numerical Methods for Optimal Control”. Advances in the Astronautical Sciences 135, No 1, pp. 497-528.

Rao, A. V., Benson, D. A., Darby, C., Patterson, M. A., Francolin, C., Sanders, I. and Huntington, G. T. (2010). “Algorithm 902: GPOPS, a MATLAB software for solving multiple-phase optimal control problems using the Gauss Pseudospectral method”. ACM Transaction of Mathematics. Software 37, 2, Article 22.

Rao, A. V., Tang, S., & Hallman, W. P. (2002). “Numerical optimization study of multiple‐ pass aeroassisted orbital transfer”. Optimal Control Applications and Methods, 23(4), pp. 215-238.

Ross, I. M. and Fahroo, F. (2002). “User’s manual for DIDO 2002: A MATLAB application package for dynamic optimization”. NPS Technical Report AA-02-002, Department of Aeronautics and Astronautics, Naval Postgraduate School, Monterey, CA. 111

Ross, I. M. and Fahroo, F. (2003). "Legendre Pseudospectral Approximations of Optimal Control Problems”. Lecture Notes in Control and Information Sciences, Vol. 295, Springer-Verlag.

Rutquist, P.E. and Edvall M.M. (2010). “Propt-Matlab Optimal Control Software”. 1. Vol. 260. Tomlab Optimization Inc.

Sakawa, Y. (1999). "Trajectory Planning of a Free-Flying Robot by Using the Optimal Control," Optimal Control Applications and Methods”, Vol. 20, pp. 235-248.

Schmidt, M. (2015). “An Interior-Point Method for Nonlinear Optimization Problems with Locatable and Separable Nonsmoothness”. EURO Journal on Computational Optimization, 3(4), pp. 309-348.

Sciarretta, L. and Guzzella, A. (2007). “Control of Hybrid Electric Vehicles”. IEEE Control Systems Magazine.

Seefelder, W. (2002). “Lunar Transfer Orbits utilizing Solar Perturbations and Ballistic Capture”. Munich Technical University, Ch. 2.6.

Shevade, S. (2012). “Numerical Optimization: Constrained Optimization – Algorithms”. Computer Science and Automation, Indian Institute of Science, India.

Stoicescu, A.P. (1995). “On Fuel-Optimal Velocity Control of a Motor Vehicle”. International Journal of Vehicle Design, Vol. 16, Nos 2/3, pp. 229-256.

Stryk, O.V. and Bulirsch, R. (1992). “Direct and Indirect Methods for Trajectory Optimization”. Annals of Operations Research, Vol. 37, Issue 1, pp 357-373.

Tassa Y., Mansard N. and Todorov E. (2014). “Control-Limited Differential Dynamic Programming”. Proceedings - IEEE International Conference on Robotics and Automation.

Terblanche, P.J., Kearney, M.P., Hearn, C.S. and Knights, P.F. (2018). “Technology Selection and Sizing of On-Board Energy Recovery Systems to Reduce Fuel Consumption of Diesel-Electric Mine Haul Trucks”. Awuah-Offei K. (eds) Energy Efficiency in the Minerals Industry. Green Energy and Technology. Springer, Cham, pp. 301-333.

112

Vanderbei, R.J. (1999). “LOQO: An Interior Point Code for Quadratic Programming”. Optimization Methods and Software, 11(1-4), pp. 451-484.

Van Keulen, T., De Jager, B., Foster, D. and Steinbuch, M. (2010a). “Velocity Trajectory Optimization in Hybrid Electric Trucks”. 2010 American Control Conference Marriott Waterfront.

Van Keulen, T., De Jager, B., Serrarens, A. and Steinbuch, M. (2010b). “Optimal Energy Management in Hybrid Electric Trucks Using Route Information”. Oil & Gas Science and Technology – Rev. IFP, Vol. 65 (2010), No. 1, pp. 103-113.

Xie, Z., Lui, C.K. and Hauser, K. (2017). “Differential Dynamic Programming with Nonlinear Constraints”. IEEE International Conference on Robotics and Automation (ICRA).

Wang, L. (2009). “Model predictive control system design and implementation using MATLAB®”. Springer Science & Business Media.

Wang, P. (2017). “Train Trajectory Optimization Methods for Energy-Efficient Railway Operations”. Delft University of Technology.

Wächter, A. and Biegler, L.T. (2006). "On the Implementation of a Primal-Dual Interior Point Filter Line Search Algorithm for Large-Scale Nonlinear Programming". Mathematical Programming 106(1), pp. 25-57.

Waltz, R. A., Morales, J. L., Nocedal, J., & Orban, D. (2006). “An Interior Algorithm for Nonlinear Optimization that Combines Line Search and Trust Region Steps”. Mathematical programming, 107(3), pp. 391-408.

Weideman, J.A.C. and Reddy S.C. (2000). “A MATLAB Differentiation Matrix Suite”. ACM Transactions on Mathematical Software, Vol. 26, No. 4, pp. 465-519.

Weinstein, M. J. and Rao, A. V. (2015). “Algorithm: ADiGator, a Toolbox for the Algorithmic Differentiation of Mathematical Functions in MATLAB Using Source Transformation via Operator Overloading”. ACM Trans. Math. Soft. V, N, Article A.

Zhao, Y. & Tsiotras, P. (2011). “Density Functions for Mesh Refinement in Numerical Optimal Control”, Journal of Guidance, Control and Dynamics, Vol. 34, No. 1, pp. 271-277.

113

Appendix A: User’s Guide

Pseudospectral OPTimiser: A software for multi-phase trajectory optimisation

114

A.1 Preface

This document serves as the user’s guide for the open-source MATLAB software Pseudospectral OPTimiser (풫풪풫풯). 풫풪풫풯 employs the Legendre-Gauss-Radau (LGR) collocation method to solve multiple-phase trajectory optimisation problems and has been designed to work with the NLP solver IPOPT.

A.2 Licensing Information

풫풪풫풯 is distributed under the MIT License. Users of the software must abide by the terms of the license.

Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.

A.3 Installation

풫풪풫풯 has been confirmed to be executable on MATLAB versions R2014a and beyond. The software can be used on any machine that runs Microsoft Windows 32-bit or 64-bit, Linux 64- bit or Mac OS-X 64-bit operating systems. MATLAB mex files for IPOPT for each of these operating systems is provided with the software. The installation procedure is:

1. Clone or download the source code from github.com/InderpreetMetla/popt. 2. Add the top-level folder to the MATLAB path or run the script poptpathsetup.m. 115

A.4 Multiple-Phase Trajectory Optimisation Problem Formulation

풫풪풫풯 is able to solve multiple-phase trajectory optimisation problems that take the following form. Let 푝 ∈ {1,…, 푃} be a single phase from a set of 푃 phases. The aim of the problem is to

() () () () () 풙 () () 풖 determine the state, 풙 푡 ∈ℝ , the control, 풖 푡 ∈ℝ , the initial time 푡 ∈ () ℝ and the final time 푡 ∈ℝ that minimise the cost function

( ) ( ) ( ) ( ) ( ) 퐽 = 휙() 푡 , 푡 , 풙() 푡 , 풙() 푡 + 푔() 푡, 풙()(푡), 풖()(푡) 푑푡 (8.1a) () subject to

() () () () () () Dynamic Equality 풙̇ (푡 ) = 풇 푡 , 풙 , 풖 Constraints

() () () () () () Inequality Path 풄 ≤ 풄 푡 , 풙 , 풖 ≤ 풄 Constraints

풃() ≤ 풃() 푡(), 푡(), 풙(), 풙() ≤ 풃() Boundary Constraints (8.1b)

() () () () () () 푡 , 푡 , 풙 푡 , 풙 푡 , … () () () Phase Linkage 휹 ≤ 휹 ≤ 휹 () () () () Constraints 풖 푡 , 풖 푡

() () Note that 푁풙 is the number of states in the 푝th phase and 푁풖 is the number of controls in the 푝th phase. Also note that 휓 ∈ {1,…, 푃 −1}, which restricts the phase linkages to be sequential.

() The functions have the following dimensions: 휙() ∈ℝ, 푔() ∈ℝ, 풇() ∈ℝ풙 , 풄() ∈

() () () () () 풄 () 풃 () 휹 ℝ , 풃 ∈ℝ and 휹 ∈ℝ , where 푁풄 and 푁풃 are the number of path () constraints and boundary conditions in phase 푝, and 푁휹 is the number of linkage constraints across link 휓.

116

A.5 Constructing a Problem

The call to 풫풪풫풯 is deceptively simple. A problem is passed and solved by the software using the syntax

The problem struct has nine possible fields, five of which are optional. The four required fields in problem are:

The five optional fields are:

 problem.derivatives: A structure with four fields (Table A.1 provides options and default settings). o .method: Specifies the derivative calculation scheme to be used. o .order: Specifies the derivative order to be used by IPOPT. o .first: If using finite-differencing or complex-step, this field can be used to set the step-size for the gradient and Jacobian approximations. o .second: This field can be used to set the step-size for the Hessian approximation. The Hessian is only approximated using finite-differencing.

117

 problem.grid: A structure with three fields (Table A.2 provides default settings). o .tol: A constant tolerance for the desired accuracy on every grid interval. o .max_refine: The maximum number of grid refinement iterations. o .phase(p): A structure with one field where (p) represents the 푝th phase of the problem. . .nodes: A structure with three fields:  .initialgrid: A row vector that contains the number of nodes in each of the intervals on the initial grid. The size of the vector designates the number of grid intervals in the initial grid. All intervals are equally spaced.  .lb: The lower bound on the number of LGR points in each new grid interval through the refinement process.  .ub: The upper bound on the number of LGR points in each new grid interval through the refinement process.

 problem.autoscale: A string that specifies whether or not to use the in-built automatic scaling scheme. Options are either ‘on’ or ‘off’ and default is ‘off’.

 problem.options: A structure that allows the user the opportunity to modify IPOPT settings. Commonly modified settings are shown in Table A.3.

Table A.1 presents the options and default settings for the problem.derivatives structure.

Table A.1: Options and default settings for problem.derivatives. Field Options Default

‘fd’ or ‘FD’ = forward difference .method ‘cs’ or ‘CS’ = complex-step ‘fd’ ‘ad’ or ‘AD’ = auto-differentiation

.order ‘1’ or ‘2’ ‘1’

.first.stepsize.gradient A positive number 3 × 10

.first.stepsize.jacobian A positive number 3 × 10

.second.stepsize.hessian A positive number 1.75 × 10

118

Note: 퓟퓞퓟퓣 uses the open-source tool MatlabAutoDiff for automatic differentiation.

Table A.2 presents the default settings for the problem.grid structure.

Table A.2: Options and default settings for problem.grid.

Field Options Default

.tol A positive number 1 × 10

.max_refine A positive integer 10

Row vector of length equal to the number of .phase(p) desired grid intervals with each element equal 4*ones(1,10) .nodes.initialgrid to number of desired LGR points.

.phase(p).nodes.lb A positive integer 3

.phase(p).nodes.ub A positive integer 10

Table A.3 presents commonly modified settings for IPOPT. Additional settings can be found in Kawajir et al. (2015).

Table A.3: Options and default settings for problem.options.

Field Options Default

.ipopt.linear_solver ‘mumps’ or ‘ma57’ ‘mumps’

.ipopt.max_iter A positive integer 3000

.ipopt.tol A positive number 1 × 10

119

A.5.1 Creating the problem.funcs Struct

There are five fields in the problem.funcs structure, with each field being a user-defined function handle. All five fields are mandatory even if the trajectory optimisation problem does not require them. The fields are

 .Dynamics: Function handle for the system dynamics,  .PathObj: Function handle for the Lagrange objective,  .PathCst: Function handle for the path constraints,  .BndObj: Function handle for the Mayer objective, and

 .BndCst: Function handle for the boundary constraints.

Note that there is no requirement for a linkage constraint function for multiple-phase problems. This is because the function is internally constructed by 풫풪풫풯 automatically.

A.5.2 Syntax for Dynamics, Path Objective and Path Constraint Functions

The dynamics, path objective and path constraint functions are all set up in a similar manner. A separate function is required for each and the syntax to define the functions is:

() matrix of size 푁 × 푁, where 푁 is the number of states for the problem and u is a matrix of

() () () size 푁 × 푁 , where 푁 is the number of controls in phase 푝.

The output of these three functions, output, changes size depending on the function. For () the dynamics function it is a matrix of size 푁 × 푁, whereas for the path objective function () () () it is a vector of length 푁 and for the path constraints it is a matrix of size 푁 × 푁 , where () 푁 is the number of path constraints in phase 푝.

120

Note: For problems that do not have a path objective, a function must be supplied with the output set to zeros(size(t)). This syntax ensures that the output has correct dimensions and the derivatives are computed correctly. Similarly, if a problem does not have path constraints, a function must still be supplied with the output set to an empty array, [].

Syntax for Boundary Objective and Boundary Constraint Functions

The boundary objective and boundary constraint functions are both set up in a similar manner. A separate function is required for each and the syntax to define the functions is:

function output = FUNCTION_NAME(t0,tf,x0,xf,auxdata)

Here 푡 and 푡 are scalars representing the initial and terminal time, respectively, and 푥 and 푥 are both a row vector of length 푁 representing the initial and terminal state. The field auxdata is the same here as it was in Section 8.1.2.

Note: For problems that do not have a boundary objective, a function must be supplied with the output set to zeros(size(t0)). This syntax ensures that the output has correct dimensions and the derivatives are computed correctly. Similarly, if a problem does not have boundary constraints, a function must still be supplied with the output set to an empty array, [].

121

Creating the problem.bounds Struct

The .phase(p) array defines the constant bounds on time, state, control and the path and boundary constraints. The 푝th element within this array of structs stores the following information:

() ×  problem.bounds.phase(p).initialstate.lb = 풙ퟎ ∈ℝ

() ×  problem.bounds.phase(p).initialstate.ub = 풙ퟎ ∈ℝ

()  problem.bounds.phase(p).state.lb = 풙 ∈ℝ×

()  problem.bounds.phase(p).state.ub = 풙 ∈ℝ×

(푝) − 1×푁푥  problem.bounds.phase(p).finalstate.lb = 풙풇 ∈ℝ

(푝) + 1×푁푥  problem.bounds.phase(p).finalstate.ub = 풙풇 ∈ℝ

1×푁(푝)  problem.bounds.phase(p).control.lb = 풖− ∈ℝ 푢

1×푁(푝)  problem.bounds.phase(p).control.ub = 풖+ ∈ℝ 푢

The .link(휓) array defines the constant bounds on the linkage constraints. The 휓th element within this array of structs stores the following information:

() ×  problem.bounds.link(휓).lb = 휹 = 휹 ∈ℝ 휹

() ×  problem.bounds.link(휓).ub = 휹 = 휹 ∈ℝ 휹

122

It is worth noting that if a problem does not have path or boundary constraints, or is not a multiple-phase problem, then the relevant field can be omitted or the lower and upper bounds can be set to the empty array. Furthermore, infinite valued bounds are not permitted and should instead be replaced with a sufficiently large number.

Creating the problem.guess Struct

The initial guess to initialise the optimisation is provided within problem.guess. This struct contains the .phase(p) field which is used to define the guess for the time, state and control in every phase of the problem. A guess for the initial and terminal time, state and control must be provided at a minimum. The limit to the number of points used to define the initial guess is set by the number of LGR points in the initial grid. The same number of points must be used in the guess for the time, state and control. The format of the guess struct is:

(푝) 푔(푝)×1  problem.guess.phase(p).time = 푡푔푢푒푠푠 ∈ℝ

() () ×  problem.guess.phase(p).state = 풙 ∈ℝ

() () () ×  problem.guess.phase(p).control = 풖 ∈ℝ where 푔() is the number of points used to define the guess and 푔() ≥2.

Problem Scaling

An in-built automatic scaling routine has been provided with 퓟퓞퓟퓣. The scaling routine uses the bounds provided by the user on the variables and then scales the variables to the domain [−1/2, 1/2].

123

Details for the Output of the Software

The two outputs from an execution of the software are the structure solution and the structure plotdata. The solution structure contains the following ten fields.

 solution.cost: The optimal objective value.

 solution.bndcost: The contribution from the Mayer objective to the total cost.

 solution.pathcost: The contribution from the Lagrange objective to the total cost.

 solution.status: Information regarding the reason IPOPT terminated.

 solution.nlp_iters: Number of IPOPT iterations.

 solution.f_evals: Number of IPOPT function evaluations.

 solution.grid_iters: The number of grid refinement iterations.

The plotdata structure contains the same information as solution.phase(p), but the results are provided at five times the number of points in order to streamline the process of creating plots.

124

Limitations of the Software

Whilst 풫풪풫풯 can solve a broad range of trajectory optimisation problems, the following restrictions are currently present:

125

Appendix B: Code for Example 1

%********************* % START MAIN SCRIPT %********************* %******************************************************************% % Infinite Horizon LQR from: % Kirk, D. E. (2012). "Optimal control theory: an introduction". % Courier Corporation, pp. 216-217. % % minimise: % \int_0^\infty (x_1^2+ 0.5*x_2^2+0.25*u^2) dt % subject to: % x_1dot = x_2 % x_2dot = 2*x_1 - x_2 + u % initial condition: % x_1(0) = -4 and x_2(0) = 4 %******************************************************************% clear;clc; t0 = -1;tf = 1;x1_0 = -4; x2_0 = 4; x1min = -10;x1max = -x1min;x2min = -10;x2max = -x2min;xf = 1; umin = -100;umax = -umin; %******************************************************************% % Setup for Problem Bounds %******************************************************************% bounds.phase.initialtime.lb = t0;bounds.phase.initialtime.ub = t0; bounds.phase.finaltime.lb = tf; bounds.phase.finaltime.ub = tf; bounds.phase.initialstate.lb = [x1_0, x2_0]; bounds.phase.initialstate.ub = [x1_0, x2_0]; bounds.phase.state.lb = [x1min, x2min]; bounds.phase.state.ub = [x1max, x2max]; bounds.phase.finalstate.lb = [x1min, x2min]; bounds.phase.finalstate.ub = [x1max, x2max]; bounds.phase.control.lb = umin; bounds.phase.control.ub = umax; %******************************************************************% % Provide Guess of Solution %******************************************************************% guess.phase.time = [t0; tf]; guess.phase.state = [[x1_0;x1max],[x2_0;x2max]]; guess.phase.control = [umin;umax]; %******************************************************************% % Setup Grid %******************************************************************% grid.phase.nodes.initialgrid = [5,5,5,5,3,2,2];grid.phase.nodes.lb = 4; grid.phase.nodes.ub = 12; grid.max_refine = 50; grid.tol = 1e-6; %******************************************************************% % Assemble Information into Problem Structure %******************************************************************% problem.name = 'lqrIH-Problem'; problem.funcs.Dynamics = @lqrIHDynamics; problem.funcs.PathObj = @lqrIHPathObj; problem.funcs.BndObj = @lqrIHBndObj; problem.funcs.PathCst = @lqrIHPathCst; problem.funcs.BndCst = @lqrIHBndCst; problem.bounds = bounds; problem.guess = guess; problem.derivatives.method = 'cs'; problem.derivatives.order = 2; problem.grid = grid; problem.autoscale = 'off'; 126

problem.options.ipopt.linear_solver = 'mumps'; %******************************************************************% % Solve Problem %******************************************************************% [solution, plotdata] = popt(problem); %******************************************************************% % Exact Solution %******************************************************************% A = [0 1; 2 -1]; B = [0;1]; C = [1 1]; D = 0; SYS = ss(A,B,C,D); Q = [2 0; 0 1]; R = 0.5; K = lqr(SYS,Q,R); %********************* % END MAIN SCRIPT %********************* %************************* % START DYNAMICS FUNCTION %************************* function output = lqrIHDynamics(t,x,u,auxdata) tau = t; x1 = x(:,1); x2 = x(:,2); dt_dtau = 2./(1-tau).^2; dx1 = dt_dtau.*x2; dx2 = dt_dtau.*(2*x1-x2+u); output = [dx1, dx2]; end %************************* % END DYNAMICS FUNCTION %************************* %************************* % START PATH OBJ FUNCTION %************************* function output = lqrIHPathObj(t,x,u,auxdata) tau = t; x1 = x(:,1); x2 = x(:,2); dt_dtau = 2./(1-tau).^2; output = dt_dtau.*(x1.^2+0.5*x2.^2+0.25*u.^2); end %************************* % END PATH OBJ FUNCTION %*************************

%************************* % START BND OBJ FUNCTION %************************* function output = lqrIHBndObj(t0,tf,x0,xf,auxdata) output = zeros(size(t0)); end %************************* % END BND OBJ FUNCTION %*************************

%************************* % START PATH CST FUNCTION %************************* function output = lqrIHPathCst(t,x,u,auxdata) output = []; end %************************* % END PATH CST FUNCTION %*************************

%************************* % START BND CST FUNCTION %************************* function output = lqrIHBndCst(t0,tf,x0,xf,auxdata) output = []; end %************************* % END BND CST FUNCTION %*************************

127

Appendix C: Code for Example 2

%********************* % START MAIN SCRIPT %********************* %******************************************************************% % Bryson Denham Problem %******************************************************************% clear;clc L = 1/9; t0 = 0; tf = 1; x0 = 0; xf = 0; xmin = 0; xmax = L; v0 = 1; vf = -v0; vmin = -10; vmax = 10; umin = -10; umax = 5; %******************************************************************% % Setup for Problem Bounds %******************************************************************% bounds.phase.initialtime.lb = t0; bounds.phase.initialtime.ub = t0; bounds.phase.finaltime.lb = tf; bounds.phase.finaltime.ub = tf; bounds.phase.initialstate.lb = [x0,v0]; bounds.phase.initialstate.ub = [x0,v0]; bounds.phase.state.lb = [xmin,vmin]; bounds.phase.state.ub = [vmax,vmax]; bounds.phase.finalstate.lb = [xf,vf]; bounds.phase.finalstate.ub = [xf,vf]; bounds.phase.control.lb = umin; bounds.phase.control.ub = umax; bounds.phase.path.lb = xmin; bounds.phase.path.ub = xmax; %******************************************************************% % Provide Guess of Solution %******************************************************************% guess.phase.time = [t0; tf]; guess.phase.state = [[x0; xf],[v0; vf]]; guess.phase.control = [umin; 0]; %******************************************************************% % Setup Grid %******************************************************************% grid.tol = 1e-10;grid.phase.nodes.lb = 4; grid.phase.nodes.ub = 10;grid.phase.nodes.initialgrid = 8*ones(1,4); %******************************************************************% % Assemble Information into Problem Structure %******************************************************************% problem.name = 'brysonDenham'; problem.funcs.Dynamics = @brysonDenhamDynamics; problem.funcs.PathObj = @brysonDenhamPathObj; problem.funcs.PathCst = @brysonDenhamPathCst; problem.funcs.BndObj = @brysonDenhamBndObj; problem.funcs.BndCst = @brysonDenhamBndCst; problem.bounds = bounds; problem.guess = guess; problem.derivatives.method = 'fd'; problem.derivatives.first.stepsize.jacobian = 1.9e-5; problem.derivatives.order = 1; problem.autoscale = 'off'; 128

problem.grid = grid; %******************************************************************% % Solve Problem %******************************************************************% [solution, plotdata] = popt(problem);

%********************* % END MAIN SCRIPT %*********************

%************************* % START DYNAMICS FUNCTION %************************* function output = brysonDenhamDynamics(t,x,u,auxdata) v = x(:,2); xdot = v; vdot = u; output = [xdot, vdot]; end %************************* % END DYNAMICS FUNCTION %*************************

%************************* % START PATH OBJ FUNCTION %************************* function output = brysonDenhamPathObj(t,x,u,auxdata) output = u.^2/2; end %************************* % END PATH OBJ FUNCTION %*************************

%************************* % START BND OBJ FUNCTION %************************* function output = brysonDenhamBndObj(t0,tf,x0,xf,auxdata) output = zeros(size(tf)); end %************************* % END BND OBJ FUNCTION %*************************

%************************* % START PATH CST FUNCTION %************************* function output = brysonDenhamPathCst(t,x,u,auxdata) output = x(:,1); end %************************* % END PATH CST FUNCTION %*************************

%************************* % START BND CST FUNCTION %************************* function output = brysonDenhamBndCst(t0,tf,x0,xf,auxdata) output = []; end %************************* % END BND CST FUNCTION %*************************

129

Appendix D: Code for Example 3

%********************* % START MAIN SCRIPT %********************* %******************************************************************% % Free Flying Robot % Sakawa, Y. (1999). "Trajectory Planning of a Free-Flying Robot % by Using the Optimal Control," Optimal Control Applications and % Methods", Vol. 20, pp. 235-248. % It is also presented in Betts as Ex 6.13 %******************************************************************% clear;clc auxdata.alpha = 0.2; auxdata.beta = 0.2; auxdata.gamma = 1; t0 = 0; tf = 12; x0 = -10; xf = 0;y0 = -10; yf = 0;theta0 = pi/2; thetaf = 0; vx0 = 0; vxf = 0;vy0 = 0; vyf = 0; omega0 = 0; omegaf = 0; xmin = -10; xmax = 10;ymin = -10; ymax = 10;thetamin = -pi; thetamax = pi; vxmin = -2; vxmax = 2;vymin = -2; vymax = 2;omegamin = -1; omegamax = 1;

%******************************************************************% % Setup for Problem Bounds and Problem Guess %******************************************************************% u1Min = 0; u1Max = 1000;u2Min = 0; u2Max = 1000;u3Min = 0; u3Max = 1000; u4Min = 0; u4Max = 1000; bounds.phase.initialtime.lb = t0; bounds.phase.initialtime.ub = t0; bounds.phase.finaltime.lb = tf; bounds.phase.finaltime.ub = tf; bounds.phase.initialstate.lb = [x0,y0,theta0,vx0,vy0,omega0]; bounds.phase.initialstate.ub = [x0,y0,theta0,vx0,vy0,omega0]; bounds.phase.state.lb = [xmin,ymin,thetamin,vxmin,vymin,omegamin]; bounds.phase.state.ub = [xmax,ymax,thetamax,vxmax,vymax,omegamax]; bounds.phase.finalstate.lb = [xf,yf,thetaf,vxf,vyf,omegaf]; bounds.phase.finalstate.ub = [xf,yf,thetaf,vxf,vyf,omegaf]; bounds.phase.control.lb = [u1Min,u2Min,u3Min,u4Min]; bounds.phase.control.ub = [u1Max,u2Max,u3Max,u4Max]; bounds.phase.path.lb = [-1000, -1000]; bounds.phase.path.ub = [1, 1]; %******************************************************************% % Setup Guess %******************************************************************% tGuess = [t0;tf];xGuess=[x0;xf];yGuess=[y0;yf];thetaGuess=[theta0;thetaf]; vxGuess= [vx0;vxf];vyGuess = [vy0;vyf]; omegaGuess=[omega0; omegaf];u1Guess=[0;0];u2Guess=[0;0];u3Guess=[0; 0]; u4Guess = [0; 0];guess.phase.time = tGuess; guess.phase.state = [xGuess,yGuess,thetaGuess,vxGuess,vyGuess,omegaGuess]; guess.phase.control = [u1Guess,u2Guess,u3Guess,u4Guess]; %******************************************************************% % Grid Information %******************************************************************% grid.tol = 1e-6;grid.phase.nodes.initialgrid = 2*ones(1,5); grid.phase.nodes.lb = 9;grid.phase.nodes.ub = 12;grid.max_refine = 25; %******************************************************************% % Assemble Information into Problem Structure %******************************************************************% problem.name = 'FFR'; problem.funcs.Dynamics = @FFRDynamics; problem.funcs.PathObj = @FFRPathObj; 130

problem.funcs.PathCst = @FFRPathCst; problem.funcs.BndObj = @FFRBndObj; problem.funcs.BndCst = @FFRBndCst; problem.bounds = bounds; problem.guess = guess; problem.auxdata = auxdata; problem.derivatives.method = 'ad'; problem.derivatives.order = 1; problem.grid = grid; problem.autoscale = 'on'; %******************************************************************% % Solve Problem %******************************************************************% [solution,plotdata]=popt(problem); %********************* % END MAIN SCRIPT %********************* %************************* % START DYNAMICS FUNCTION %************************* function output = FFRDynamics(t,x,u,auxdata) alpha = auxdata.alpha;beta = auxdata.beta; y1 = x(:,1);y2 = x(:,2);y3 = x(:,3);y4 = x(:,4);y5 = x(:,5);y6 = x(:,6); u1 = u(:,1);u2 = u(:,2);u3 = u(:,3);u4 = u(:,4);T1 = u1-u2;T2 = u3-u4; y1dot = y4; y2dot = y5; y3dot= y6;y4dot = (T1+T2).*cos(y3); y5dot = (T1+T2).*sin(y3);y6dot = alpha*T1-beta*T2; output = [y1dot, y2dot, y3dot, y4dot, y5dot, y6dot]; end %************************* % END DYNAMICS FUNCTION %************************* %************************* % START PATH OBJ FUNCTION %************************* function output = FFRPathObj(t,x,u,auxdata) gamma = auxdata.gamma; u1 = u(:,1);u2 = u(:,2);u3 = u(:,3);u4 = u(:,4); output = (u1+u2+u3+u4).*gamma; end %************************* % END PATH OBJ FUNCTION %************************* %************************* % START BND OBJ FUNCTION %************************* function output = FFRBndObj(t0,tf,x0,xf,auxdata) output = zeros(size(t0)); end %************************* % END BND OBJ FUNCTION %************************* %************************* % START PATH CST FUNCTION %************************* function output = FFRPathCst(t,x,u,auxdata) u1 = u(:,1);u2 = u(:,2);u3 = u(:,3);u4 = u(:,4); output = [u1+u2, u3+u4]; end %************************* % END PATH CST FUNCTION %************************* %************************* % START BND CST FUNCTION %************************* function output = FFRBndCst(t0,tf,x0,xf,auxdata) output = []; end %************************* % END BND CST FUNCTION %*************************

131

Appendix E: Code for Example 4

%********************* % START MAIN SCRIPT %********************* %******************************************************************% % Lee-Ramirez bioreactor %******************************************************************% clear;clc t0 = 0;tf = 10; x10min = 1; x1min = 0; x1fmin = 0;x20min = 0.1; x2min = 0; x2fmin = 0; x30min = 40; x3min = 0; x3fmin = 0;x40min = 0; x4min = 0; x4fmin = 0; x50min = 0; x5min = 0; x5fmin = 0;x60min = 1; x6min = 0; x6fmin = 0; x70min = 0; x7min = 0; x7fmin = 0;x80min = 0; x8min = 0; x8fmin = 0; x90min = 0; x9min = 0; x9fmin = 0;x10max = 1; x1max = 8; x1fmax = 8; x20max = 0.1; x2max = 8; x2fmax = 8;x30max = 40; x3max = 45; x3fmax = 45; x40max = 0; x4max = 8; x4fmax = 8;x50max = 0; x5max = 8; x5fmax = 8; x60max = 1; x6max = 8; x6fmax = 8;x70max = 0; x7max = 8; x7fmax = 8; x80max = 1; x8max = 1; x8fmax = 1;x90max = 1; x9max = 1; x9fmax = 1; u1min = -10;u1max = 10;u2min = -10;u2max = 10; %******************************************************************% % Setup for Problem Bounds %******************************************************************% bounds.phase.initialtime.lb = t0; bounds.phase.initialtime.ub = t0; bounds.phase.finaltime.lb = tf; bounds.phase.finaltime.ub = tf; bounds.phase.initialstate.lb = [x10min,x20min,x30min,x40min,x50min,x60min,x70min,x80min,x90min]; bounds.phase.initialstate.ub = [x10max,x20max,x30max,x40max,x50max,x60max,x70max,x80max,x90max]; bounds.phase.state.lb = [x1min,x2min,x3min,x4min,x5min,x6min,x7min,x8min,x9min]; bounds.phase.state.ub = [x1max,x2max,x3max,x4max,x5max,x6max,x7max,x8max,x9max]; bounds.phase.finalstate.lb = [x1fmin,x2fmin,x3fmin,x4fmin,x5fmin,x6fmin,x7fmin,x8fmin,x9fmin]; bounds.phase.finalstate.ub = [x1fmax,x2fmax,x3fmax,x4fmax,x5fmax,x6fmax,x7fmax,x8fmax,x9fmax]; bounds.phase.control.lb = [u1min,u2min]; bounds.phase.control.ub = [u1max,u2max]; %******************************************************************% % Provide Guess of Solution %******************************************************************% guess.phase.time = [t0; tf]; guess.phase.state = [[1;4],[0.1;7],[40;40],[0;1],[0;1],[1;1],[0;0],[0;1],[0;1]]; guess.phase.control = [[0;0],[0;0]]; %******************************************************************% % Setup Grid %******************************************************************% grid.phase.nodes.initialgrid = [20, 35, 50]; grid.phase.nodes.lb = 4; grid.phase.nodes.ub = 12; grid.tol = 1e-3; grid.max_iter = 15; %******************************************************************% % Assemble Information into Problem Structure %******************************************************************% problem.name = 'leeBioReactor-Problem'; problem.funcs.Dynamics = @leeBioReactorDynamics; problem.funcs.PathObj = @leeBioReactorPathObj; problem.funcs.BndObj = @leeBioReactorBndObj; 132

problem.funcs.PathCst = @leeBioReactorPathCst; problem.funcs.BndCst = @leeBioReactorBndCst; problem.bounds = bounds; problem.guess = guess; problem.derivatives.method = 'ad'; problem.derivatives.order = 2; problem.grid = grid; problem.autoscale = 'off'; %******************************************************************% % Solve Problem %******************************************************************% [solution, plotdata] = popt(problem); %********************* % END MAIN SCRIPT %********************* %************************* % START DYNAMICS FUNCTION %************************* function output = leeBioReactorDynamics(t,x,u,auxdata) s = x; x1 = s(:,1);x2 = s(:,2);x3 = s(:,3);x4 = s(:,4); x5 = s(:,5);x6 = s(:,6);x7 = s(:,7);u1 = s(:,8);u2 = s(:,9); k1 = 0.09*x5./(0.034+x5); g1 = (x3./(14.35+x3.*(1+x3./111.5))).*(x6+(0.22*x7)./(0.22+x5)); Rfp = (0.233*x3./(14.35+x3.*(1+x3./111.5))).*((0.0005+x5)./(0.022+x5)); x1dot = u1+u2;x2dot = g1.*x2-(u1+u2).*x2./x1; x3dot = 100*u1./x1-(u1+u2).*x3./x1-g1.*x2/0.51;x4dot = Rfp.*x2-(u1+u2).*x4./x1; x5dot = 4*u2./x1-(u1+u2).*x5./x1;x6dot = -k1.*x6;x7dot = k1.*(1-x7); x8dot = u(:,1);x9dot = u(:,2); output = [x1dot x2dot x3dot x4dot x5dot x6dot x7dot x8dot x9dot]; end %************************* % END DYNAMICS FUNCTION %************************* %************************* % START PATH OBJ FUNCTION %************************* function output = leeBioReactorPathObj(t,x,u,auxdata) N = length(t);rho = 0.1/N;u1dot = u(:,1); u2dot = u(:,2); output = rho.*((u1dot).^2+(u2dot).^2); end %************************* % END PATH OBJ FUNCTION %************************* %************************* % START BND OBJ FUNCTION %************************* function output = leeBioReactorBndObj(t0,tf,x0,xf,auxdata) x1f = xf(1); x4f = xf(4); output = -x1f.*x4f; end %************************* % END BND OBJ FUNCTION %************************* %************************* % START PATH CST FUNCTION %************************* function output = leeBioReactorPathCst(t,x,u,auxdata) output = [];end %************************* % END PATH CST FUNCTION %************************* %************************* % START BND CST FUNCTION %************************* function output = leeBioReactorBndCst(t0,tf,x0,xf,auxdata) output = [];end %************************* % END BND CST FUNCTION %*************************

133

Appendix F: Code for Example 5

%********************* % START MAIN SCRIPT %********************* %******************************************************************% % Goddard Rocket Problem % This problem is taken from the following reference: % Bryson, A. and Ho, Y. (1975). “Applied Optimal Control”. %******************************************************************% clear;clc h0 = 0;v0 = 0;m0 = 3;mf = 1;hmin = 0;hmax = 25000; vmin = 0;vmax = 1000;mmin = 0.5;mmax = m0;t0 = 0;tfmin = 0;tfmax = 50; constants.g = 32.174; constants.H = 23800; constants.c = sqrt(3.264* constants.g*constants.H); Tmax = 193.044; constants.drag_coeff = 5.49153485*10^(-5); %******************************************************************% % Setup for Problem Bounds and Problem Guess %******************************************************************% %*********% % Phase 1 %*********% iphase = 1; bounds.phase(iphase).initialtime.lb = t0; bounds.phase(iphase).initialtime.ub = t0; bounds.phase(iphase).finaltime.lb = tfmin; bounds.phase(iphase).finaltime.ub = tfmax; bounds.phase(iphase).initialstate.lb = [h0, v0, m0]; bounds.phase(iphase).initialstate.ub = [h0, v0, m0]; bounds.phase(iphase).state.lb = [hmin, vmin, mmin]; bounds.phase(iphase).state.ub = [hmax, vmax, mmax]; bounds.phase(iphase).finalstate.lb = [hmin, vmin, mmin]; bounds.phase(iphase).finalstate.ub = [hmax, vmax, mmax]; bounds.phase(iphase).control.lb = Tmax;bounds.phase(iphase).control.ub = Tmax; bounds.phase(iphase).path.lb = [];bounds.phase(iphase).path.ub = []; bounds.phase(iphase).boundary.lb = 0;bounds.phase(iphase).boundary.ub = 1000; guess.phase(iphase).time = [t0; tfmax]; guess.phase(iphase).state(:,1) = [h0; h0]; guess.phase(iphase).state(:,2) = [v0; v0]; guess.phase(iphase).state(:,3) = [m0; mf];guess.phase(iphase).control = [0; Tmax]; %*********% % Phase 2 %*********% iphase = 2; bounds.phase(iphase).initialtime.lb = tfmin; bounds.phase(iphase).initialtime.ub = tfmax; bounds.phase(iphase).finaltime.lb = tfmin; bounds.phase(iphase).finaltime.ub = tfmax; bounds.phase(iphase).initialstate.lb = [hmin, vmin, mmin]; bounds.phase(iphase).initialstate.ub = [hmax, vmax, mmax]; bounds.phase(iphase).state.lb = [hmin, vmin, mmin]; bounds.phase(iphase).state.ub = [hmax, vmax, mmax]; bounds.phase(iphase).finalstate.lb = [hmin, vmin, mmin]; bounds.phase(iphase).finalstate.ub = [hmax, vmax, mmax]; bounds.phase(iphase).control.lb = 0;bounds.phase(iphase).control.ub = Tmax; bounds.phase(iphase).path.lb = 0;bounds.phase(iphase).path.ub = 0; bounds.phase(iphase).boundary.lb = [0,0]; bounds.phase(iphase).boundary.ub = [0,1000]; guess.phase(iphase).time = [tfmin; tfmax]; guess.phase(iphase).state(:,1) = [h0; hmax]; 134

guess.phase(iphase).state(:,2) = [vmax; vmax]; guess.phase(iphase).state(:,3) = [m0; mf]; guess.phase(iphase).control(:,1) = [0; Tmax]; %*********% % Phase 3 %*********% iphase = 3; bounds.phase(iphase).initialtime.lb = tfmin; bounds.phase(iphase).initialtime.ub = tfmax; bounds.phase(iphase).finaltime.lb = tfmin; bounds.phase(iphase).finaltime.ub = tfmax; bounds.phase(iphase).initialstate.lb = [hmin, vmin, mmin]; bounds.phase(iphase).initialstate.ub = [hmax, vmax, mmax]; bounds.phase(iphase).state.lb = [hmin, vmin, mmin]; bounds.phase(iphase).state.ub = [hmax, vmax, mmax]; bounds.phase(iphase).finalstate.lb = [hmin, vmin, mf]; bounds.phase(iphase).finalstate.ub = [hmax, vmax, mf]; bounds.phase(iphase).control.lb = 0;bounds.phase(iphase).control.ub = 0; bounds.phase(iphase).path.lb = [];bounds.phase(iphase).path.ub = []; bounds.phase(iphase).boundary.lb = 0;bounds.phase(iphase).boundary.ub = 1000; guess.phase(iphase).time = [tfmin; tfmax]; guess.phase(iphase).state(:,1) = [hmin; hmax]; guess.phase(iphase).state(:,2) = [vmax; vmax]; guess.phase(iphase).state(:,3) = [m0; mf]; guess.phase(iphase).control(:,1) = [0; Tmax]; %******************************************************************% % Linkage Bounds %******************************************************************% ilink = 1; bounds.link(ilink).lb = [0,0,0,0];bounds.link(ilink).ub = [0,0,0,0]; ilink = 2; bounds.link(ilink).lb = [0,0,0,0];bounds.link(ilink).ub = [0,0,0,0]; %******************************************************************% % Setup Grid %******************************************************************% grid.phase(1).nodes.initialgrid = 12*ones(1,2); grid.phase(2).nodes.initialgrid = 12*ones(1,2); grid.phase(3).nodes.initialgrid = 12*ones(1,2); grid.tol = 1e-10; %******************************************************************% % Assemble Information into Problem Structure %******************************************************************% problem.name = 'goddardRocket'; problem.funcs.Dynamics = @goddardRocketDynamics; problem.funcs.PathObj = @goddardRocketPathObj; problem.funcs.PathCst = @goddardRocketPathCst; problem.funcs.BndObj = @goddardRocketBndObj; problem.funcs.BndCst = @goddardRocketBndCst; problem.bounds = bounds; problem.guess = guess; problem.auxdata = auxdata; problem.derivatives.method = 'ad'; problem.derivatives.order = 1; problem.grid = grid; problem.autoscale = 'on'; %******************************************************************% % Solve Problem %******************************************************************% [solution, plotdata] = popt(problem); %********************* % END MAIN SCRIPT %********************* %************************* % START DYNAMICS FUNCTION %************************* function output = goddardRocketDynamics(t,x,u,auxdata) h = x(:,1);v = x(:,2);m = x(:,3); D = auxdata.drag_coeff.*(v.^2).*exp(-h/auxdata.H); hdot = v;vdot = (u-D)./m-auxdata.g*ones(size(t));mdot = -u./auxdata.c; output = [hdot, vdot, mdot];end 135

%************************* % END DYNAMICS FUNCTION %************************* %************************* % START PATH OBJ FUNCTION %************************* function output = goddardRocketPathObj(t,x,u,auxdata) output = zeros(size(t)); end %************************* % END PATH OBJ FUNCTION %************************* %************************* % START BND OBJ FUNCTION %************************* function output = goddardRocketBndObj(t0,tf,x0,xf,auxdata) iphase = auxdata.iphase; if iphase == 1 || iphase == 2 output = zeros(size(tf)); else output = -xf(1); end end %************************* % END BND OBJ FUNCTION %************************* %************************* % START PATH CST FUNCTION %************************* function output = goddardRocketPathCst(t,x,u,auxdata) iphase = auxdata.iphase; if iphase == 1 || iphase == 3 output = []; elseif iphase == 2 T = u; h = x(:,1); v = x(:,2); m = x(:,3); D = auxdata.drag_coeff.*(v.^2).*exp(-h/auxdata.H); output = T-D-m*auxdata.g-((auxdata.c^2).*(ones(size(t))+ v/auxdata.c)./... (auxdata.H*auxdata.g)-ones(size(t))-2./(v/... auxdata.c)).*(m*auxdata.g./(ones(size(t))+4./(v/auxdata.c)+2./... ((v/auxdata.c).^2))); end end %************************* % END PATH CST FUNCTION %************************* %************************* % START BND CST FUNCTION %************************* function output = goddardRocketBndCst(t0,tf,x0,xf,auxdata) iphase = auxdata.iphase; if iphase == 1 || iphase == 3 output = tf - t0; elseif iphase == 2 h = x0(1); v = x0(2); m = x0(3); D = auxdata.drag_coeff.*(v.^2).*exp(-h/auxdata.H); output = [m*auxdata.g-(1+v/auxdata.c).*D,tf-t0]; end end %************************* % END BND CST FUNCTION %*************************

136

Appendix G: Code for HTEMCS

%********************* % START MAIN SCRIPT %********************* %******************************************************************% % Haul Truck Phase 1 %******************************************************************% clear all; clc %******************************************************************% % Auxdata %******************************************************************% auxdata.eta1 = 0.8075;auxdata.eta2 = 0.765;auxdata.eta3 = 0.765; auxdata.Ecap = 166e6;auxdata.mu = 0.02;auxdata.g = 9.81; auxdata.cd = 0.9;auxdata.rho = 1.225;auxdata.A = 70; Amax = 0.8;Amin = -Amax; %******************************************************************% % Problem Limits %******************************************************************% t0 = 0;tf = 35;SOE_lb = 0;SOE_ub = 1;dist_lb = 0;dist_ub = 50; v_lb = 0.01;v_ub = 15/3.6;rimpull_max = rimpull(18);rimpull_min = 0; brake_min = dynbrake(8.2043);brake_max = 0;motor_max = 1490e3; motor_min = 0; %******************************************************************% % Setup for Problem Bounds %******************************************************************% % Phase 1 bounds.phase.initialtime.lb = t0;bounds.phase.initialtime.ub = t0; bounds.phase.finaltime.lb = 0.1;bounds.phase.finaltime.ub = tf; bounds.phase.initialstate.lb = [SOE_lb,dist_lb,v_lb]; bounds.phase.initialstate.ub = [SOE_lb,dist_lb,v_lb]; bounds.phase.state.lb = [SOE_lb,dist_lb,v_lb]; bounds.phase.state.ub = [SOE_ub,dist_ub,v_ub]; bounds.phase.finalstate.lb = [SOE_lb,dist_ub,v_lb]; bounds.phase.finalstate.ub = [SOE_ub,dist_ub,v_ub]; bounds.phase.control.lb = [rimpull_min, motor_min, brake_max]; bounds.phase.control.ub = [rimpull_max, motor_max, brake_max]; bounds.phase.path.lb = [-rimpull_max,brake_max,0.5*Amin*2]; bounds.phase.path.ub = [rimpull_min,-brake_min,0.5*Amax*2]; bounds.phase.boundary.lb = [];bounds.phase.boundary.ub = []; %******************************************************************% % Provide Problem Guess %******************************************************************% x1guess = [SOE_lb;SOE_lb;SOE_lb]; x2guess = [dist_lb;dist_ub/2;dist_ub]; x3guess = [v_lb;v_ub;v_ub];u1guess = [0;.75*rimpull_max;rimpull_max]; u2guess = [motor_min;motor_min;motor_min]; u3guess = [brake_max;brake_max;brake_max];tguess = [t0;tf/2;.9*tf]; guess.phase.state = [x1guess,x2guess,x3guess]; guess.phase.control=[u1guess,u2guess,u3guess];guess.phase.time=tguess; %******************************************************************% % Setup Grid %******************************************************************% grid.phase.nodes.initialgrid = 8*ones(1,2);grid.phase.nodes.lb = 3; grid.phase.nodes.ub = 12;grid.max_refine = 15;grid.tol = 1e-3; %******************************************************************% % Assemble Information into Problem Structure %******************************************************************% problem.name = 'Haul_Truck1'; problem.funcs.Dynamics = @HaulTruckDynamics1; 137

problem.funcs.PathObj = @HaulTruckPathObj1; problem.funcs.BndObj = @HaulTruckBndObj1; problem.funcs.PathCst = @HaulTruckPathCst1; problem.funcs.BndCst = @HaulTruckBndCst1; problem.auxdata = auxdata; problem.bounds = bounds; problem.guess = guess; problem.grid = grid; problem.options.ipopt.max_iter = 2000; problem.derivatives.method = 'cs'; problem.derivatives.order = 1; problem.autoscale = 'on'; %******************************************************************% % Solve Problem %******************************************************************% solution1 = popt(problem); %******************************************************************% % Haul Truck Phase 2 %******************************************************************% clc; clear problem %******************************************************************% % Problem Limits %******************************************************************% t0 = solution1.phase.time(end);add_time = 150;tf = t0+add_time; SOE_lb = 0;SOE_ub = 1;SOE_start = solution1.phase.state(end,1); dist_lb = solution1.phase.state(end,2);dist_ub = dist_lb+503; v_lb = 0.01;v_ub = 30/3.6; v_start = solution1.phase.state(end,3); rimpull_max = rimpull(18);rimpull_min = 0;brake_min = dynbrake(8.2043); brake_max = 0;motor_min = 0; %******************************************************************% % Setup for Problem Bounds %******************************************************************% bounds.phase.initialtime.lb = t0;bounds.phase.initialtime.ub = t0; bounds.phase.finaltime.lb = t0+0.1;bounds.phase.finaltime.ub = tf; bounds.phase.initialstate.lb = [SOE_start,dist_lb,v_start]; bounds.phase.initialstate.ub = [SOE_start,dist_lb,v_start]; bounds.phase.state.lb = [SOE_lb,dist_lb,v_lb]; bounds.phase.state.ub = [SOE_ub,dist_ub,v_ub]; bounds.phase.finalstate.lb = [SOE_lb,dist_ub,v_lb]; bounds.phase.finalstate.ub = [SOE_ub,dist_ub,v_ub]; bounds.phase.control.lb = [rimpull_min, motor_min, brake_min]; bounds.phase.control.ub = [rimpull_max, motor_max, brake_max]; bounds.phase.path.lb = [-rimpull_max, brake_max, Amin]; bounds.phase.path.ub = [rimpull_min, -brake_min, Amax]; bounds.phase.boundary.lb = [];bounds.phase.boundary.ub = []; %******************************************************************% % Provide Problem Guess %******************************************************************% x1guess = [SOE_start;0.5;0.8]; x2guess = [dist_lb;dist_ub/2;dist_ub]; x3guess = [v_start;v_ub;v_ub]; u1guess = [rimpull_min;rimpull_min;rimpull_min]; u2guess = [motor_min;motor_min;motor_min]; u3guess = [-700e3;-700e3;-700e3]; tguess = [0;0.3*add_time;0.8*add_time]+t0; guess.phase.state = [x1guess,x2guess,x3guess]; guess.phase.control = [u1guess,u2guess,u3guess]; guess.phase.time = tguess; %******************************************************************% % Setup Grid %******************************************************************% grid.phase.nodes.initialgrid = 4*ones(1,15);grid.phase.nodes.lb = 3; grid.phase.nodes.ub = 12;grid.max_refine = 15;grid.tol = 1e-3; %******************************************************************% % Assemble Information into Problem Structure %******************************************************************% problem.name = 'Haul_Truck2'; problem.funcs.Dynamics = @HaulTruckDynamics2; problem.funcs.PathObj = @HaulTruckPathObj2; problem.funcs.BndObj = @HaulTruckBndObj2; problem.funcs.PathCst = @HaulTruckPathCst2; problem.funcs.BndCst = @HaulTruckBndCst2; problem.auxdata = auxdata; 138

problem.bounds = bounds; problem.guess = guess; problem.grid = grid; problem.options.ipopt.max_iter = 10000; problem.derivatives.method = 'cs'; problem.derivatives.order = 1; problem.autoscale = 'on'; %******************************************************************% % Solve Problem %******************************************************************% solution2 = popt(problem); %******************************************************************% % Haul Truck Phase 3 %******************************************************************% clc; clear problem %******************************************************************% % Problem Limits %******************************************************************% t0 = solution2.phase.time(end);add_time = 40;tf = t0+add_time; SOE_lb = 0;SOE_ub = 1;SOE_start = solution2.phase.state(end,1); dist_lb = solution2.phase.state(end,2); dist_ub = dist_lb+100;v_lb = 0.01;v_ub = 30/3.6; v_start = solution2.phase.state(end,3); rimpull_max = rimpull(18); rimpull_min = 0;brake_min = dynbrake(8.2043);brake_max = 0; motor_max = 1490e3;motor_min = 0; %******************************************************************% % Setup for Problem Bounds %******************************************************************% bounds.phase.initialtime.lb = t0;bounds.phase.initialtime.ub = t0; bounds.phase.finaltime.lb = t0+0.1;bounds.phase.finaltime.ub = tf; bounds.phase.initialstate.lb = [SOE_start,dist_lb,v_start]; bounds.phase.initialstate.ub = [SOE_start,dist_lb,v_start]; bounds.phase.state.lb = [SOE_lb,dist_lb,v_lb]; bounds.phase.state.ub = [SOE_ub,dist_ub,v_ub]; bounds.phase.finalstate.lb = [SOE_lb,dist_ub,v_lb]; bounds.phase.finalstate.ub = [SOE_ub,dist_ub,v_ub/2]; bounds.phase.control.lb = [rimpull_min, motor_min, brake_min]; bounds.phase.control.ub = [rimpull_max, motor_max, brake_max]; bounds.phase.path.lb = [-rimpull_max, brake_max, Amin]; bounds.phase.path.ub = [rimpull_min, -brake_min, Amax]; bounds.phase.boundary.lb = [];bounds.phase.boundary.ub = []; %******************************************************************% % Provide Problem Guess %******************************************************************% x1guess = [SOE_start;0.5;0.5]; x2guess = [dist_lb;dist_ub/2;dist_ub]; x3guess = [v_start;v_ub/1.5;v_ub/2]; u1guess = [rimpull_min;rimpull_min;rimpull_min]; u2guess = [motor_min;motor_min;motor_min]; u3guess = [0;-700e3;-700e3]; tguess = [0;0.2*add_time;0.95*add_time]+t0; guess.phase.state = [x1guess,x2guess,x3guess]; guess.phase.control = [u1guess,u2guess,u3guess]; guess.phase.time = tguess; %******************************************************************% % Setup Grid %******************************************************************% grid.phase.nodes.initialgrid = 8;grid.phase.nodes.lb = 3; grid.phase.nodes.ub = 12;grid.max_refine = 15;grid.tol = 1e-2; %******************************************************************% % Assemble Information into Problem Structure %******************************************************************% problem.name = 'Haul_Truck3'; problem.funcs.Dynamics = @HaulTruckDynamics3; problem.funcs.PathObj = @HaulTruckPathObj3; problem.funcs.BndObj = @HaulTruckBndObj3; problem.funcs.PathCst = @HaulTruckPathCst3; problem.funcs.BndCst = @HaulTruckBndCst3; problem.auxdata = auxdata; problem.bounds = bounds; problem.guess = guess; 139

problem.grid = grid; problem.options.ipopt.max_iter = 10000; problem.derivatives.method = 'cs'; problem.derivatives.order = 1; problem.autoscale = 'on'; %******************************************************************% % Solve Problem %******************************************************************% solution3 = popt(problem); %******************************************************************% % Haul Truck Phase 4 %******************************************************************% clc; clear problem %******************************************************************% % Problem Limits %******************************************************************% t0 = solution3.phase.time(end);add_time = 190;tf = t0+add_time; SOE_lb = 0;SOE_ub = 1;SOE_start = solution3.phase.state(end,1); dist_lb = solution3.phase.state(end,2);dist_ub = dist_lb+503; v_lb = 0.01;v_ub = 30/3.6;v_start = solution3.phase.state(end,3); rimpull_max = 1325.8e3;rimpull_min = 0;brake_min = dynbrake(8.2043); brake_max = 0;motor_max = 150e3;motor_min = 0; %******************************************************************% % Setup for Problem Bounds %******************************************************************% bounds.phase.initialtime.lb = t0;bounds.phase.initialtime.ub = t0; bounds.phase.finaltime.lb = t0+0.1;bounds.phase.finaltime.ub = tf; bounds.phase.initialstate.lb = [SOE_start,dist_lb,v_start]; bounds.phase.initialstate.ub = [SOE_start,dist_lb,v_start]; bounds.phase.state.lb = [SOE_lb,dist_lb,v_lb]; bounds.phase.state.ub = [SOE_ub,dist_ub,v_ub]; bounds.phase.finalstate.lb = [SOE_lb,dist_ub,v_lb+2.5]; bounds.phase.finalstate.ub = [SOE_ub,dist_ub,v_ub/2]; bounds.phase.control.lb = [rimpull_min, motor_min+120e3, brake_max]; bounds.phase.control.ub = [rimpull_max, motor_max, brake_max]; bounds.phase.path.lb = [-rimpull_max, brake_max, Amin]; bounds.phase.path.ub = [rimpull_min, -brake_min, Amax]; bounds.phase.boundary.lb = [];bounds.phase.boundary.ub = []; %******************************************************************% % Provide Problem Guess %******************************************************************% x1guess = [SOE_start;0.5;0.3]; x2guess = [dist_lb;dist_ub/2;dist_ub]; x3guess = [v_start;v_ub/2;v_ub/2]; u1guess = [600e3;600e3;600e3];u2guess = [100e3;100e3;100e3]; u3guess = [brake_max;brake_max;brake_max]; tguess = [0;0.5*add_time;0.9*add_time]+t0; guess.phase.state = [x1guess,x2guess,x3guess]; guess.phase.control = [u1guess,u2guess,u3guess]; guess.phase.time = tguess; %******************************************************************% % Setup Grid %******************************************************************% grid.phase.nodes.initialgrid = 8*ones(1,10);grid.phase.nodes.lb = 3; grid.phase.nodes.ub = 12;grid.max_refine = 5;grid.tol = 2e-3; %******************************************************************% % Assemble Information into Problem Structure %******************************************************************% problem.name = 'Haul_Truck4'; problem.funcs.Dynamics = @HaulTruckDynamics4; problem.funcs.PathObj = @HaulTruckPathObj4; problem.funcs.BndObj = @HaulTruckBndObj4; problem.funcs.PathCst = @HaulTruckPathCst4; problem.funcs.BndCst = @HaulTruckBndCst4; problem.auxdata = auxdata; problem.bounds = bounds; problem.guess = guess; problem.grid = grid; problem.options.ipopt.max_iter = 10000; problem.derivatives.method = 'cs'; problem.derivatives.order = 1; 140

problem.autoscale = 'on'; %******************************************************************% % Solve Problem %******************************************************************% solution4 = popt(problem); %******************************************************************% % Haul Truck Phase 5 %******************************************************************% clc; clear problem %******************************************************************% % Problem Limits %******************************************************************% t0 = solution4.phase.time(end);add_time = 15;tf = t0+add_time; SOE_lb = 0;SOE_ub = 1;SOE_start = solution4.phase.state(end,1); dist_lb = solution4.phase.state(end,2);dist_ub = dist_lb+50; v_lb = 0.01;v_ub = 15/3.6;v_start = solution4.phase.state(end,3); rimpull_max=rimpull(18);rimpull_min=0;brake_min=dynbrake(8.2043); brake_max = 0;motor_max = 1490e3;motor_min = 0; %******************************************************************% % Setup for Problem Bounds %******************************************************************% bounds.phase.initialtime.lb = t0;bounds.phase.initialtime.ub = t0; bounds.phase.finaltime.lb = t0+0.1;bounds.phase.finaltime.ub = tf; bounds.phase.initialstate.lb = [SOE_start,dist_lb,v_start]; bounds.phase.initialstate.ub = [SOE_start,dist_lb,v_start]; bounds.phase.state.lb = [SOE_lb,dist_lb,v_lb]; bounds.phase.state.ub = [SOE_ub,dist_ub,v_ub]; bounds.phase.finalstate.lb = [SOE_lb,dist_ub,v_lb]; bounds.phase.finalstate.ub = [SOE_ub,dist_ub,v_lb]; bounds.phase.control.lb = [rimpull_min, motor_min, brake_min]; bounds.phase.control.ub = [rimpull_max, motor_max, brake_max]; bounds.phase.path.lb = [-rimpull_max, brake_max, Amin*4]; bounds.phase.path.ub = [rimpull_min, -brake_min, Amax*4]; bounds.phase.boundary.lb = [];bounds.phase.boundary.ub = []; %******************************************************************% % Provide Problem Guess %******************************************************************% x1guess = [SOE_start;0;0.1]; x2guess = [dist_lb;dist_ub/2;dist_ub]; x3guess = [v_start;v_ub/2;v_lb]; u1guess = [rimpull_min;rimpull_min;rimpull_min]; u2guess = [motor_max;motor_min;motor_min];u3guess = [0;0;0]; tguess = [0;0.5*add_time;0.8*add_time]+t0; guess.phase.state = [x1guess,x2guess,x3guess]; guess.phase.control = [u1guess,u2guess,u3guess]; guess.phase.time = tguess; %******************************************************************% % Setup Grid %******************************************************************% grid.phase.nodes.initialgrid = 8*ones(1,10);grid.phase.nodes.lb = 3; grid.phase.nodes.ub = 12;grid.max_refine = 15;grid.tol = 1e-3; %******************************************************************% % Assemble Information into Problem Structure %******************************************************************% problem.name = 'Haul_Truck5'; problem.funcs.Dynamics = @HaulTruckDynamics5; problem.funcs.PathObj = @HaulTruckPathObj5; problem.funcs.BndObj = @HaulTruckBndObj5; problem.funcs.PathCst = @HaulTruckPathCst5; problem.funcs.BndCst = @HaulTruckBndCst5; problem.auxdata = auxdata; problem.bounds = bounds; problem.guess = guess; problem.grid = grid; problem.options.ipopt.max_iter = 10000; problem.derivatives.method = 'cs'; problem.derivatives.order = 1; problem.autoscale = 'on'; %******************************************************************% % Solve Problem %******************************************************************% 141

solution5 = popt(problem); %********************* % END MAIN SCRIPT %*********************

%************************* % START DYNAMICS FUNCTION %************************* function output = HaulTruckDynamics1(t,x,u,auxdata) eta1 = auxdata.eta1;eta2 = auxdata.eta2;eta3 = auxdata.eta3; Ecap = auxdata.Ecap;mu = auxdata.mu;g = auxdata.g;Cd = auxdata.cd; rho = auxdata.rho;A = auxdata.A;mass = 250e3;x3 = x(:,3);u1 = u(:,1); u2 = u(:,2);u3 = u(:,3);G = 0;Fr = mu*mass*g;Fg = G*mass*g; Fa = 0.5*Cd*rho*A*x3.^2;alpha = 1;beta = 0;gamma = 0; Fres = Fr + Fg + Fa; Power = alpha*eta1*u1+beta*eta2*u2+gamma*eta3*u3;Ft = Power./x3; x1dot = -(u2/eta2+eta3*u3)/Ecap;x2dot=x3;x3dot=(Ft-Fres)./mass; output = [x1dot,x2dot,x3dot]; end function output = HaulTruckDynamics2(t,x,u,auxdata) eta1 = auxdata.eta1;eta2 = auxdata.eta2;eta3 = auxdata.eta3; Ecap = auxdata.Ecap;mu = auxdata.mu;g = auxdata.g;Cd = auxdata.cd; rho = auxdata.rho;A = auxdata.A;mass = 250e3;x3 = x(:,3); u1 = u(:,1);u2 = u(:,2);u3 = u(:,3);G = -0.1;Fr = mu*mass*g; Fg = G*mass*g;Fa = 0.5*Cd*rho*A*x3.^2;alpha = 0;beta = 0; gamma = 1;Fres = Fr + Fg + Fa; Power = alpha*eta1*u1+beta*eta2*u2+gamma*eta3*u3;Ft = Power./x3; x1dot = -(u2/eta2+eta3*u3)/Ecap;x2dot = x3;x3dot=(Ft-Fres)./mass; output = [x1dot,x2dot,x3dot]; end function output = HaulTruckDynamics3(t,x,u,auxdata) eta1 = auxdata.eta1;eta2 = auxdata.eta2;eta3 = auxdata.eta3; Ecap = auxdata.Ecap;mu = auxdata.mu;g = auxdata.g;Cd = auxdata.cd; rho = auxdata.rho;A = auxdata.A;mass = 250e3;x3 = x(:,3);u1 = u(:,1); u2 = u(:,2);u3 = u(:,3);G = 0;Fr = mu*mass*g;Fg = G*mass*g; Fa = 0.5*Cd*rho*A*x3.^2;alpha = 1;beta = 1;gamma = 1; Fres = Fr + Fg + Fa; Power = alpha*eta1*u1+beta*eta2*u2+gamma*eta3*u3;Ft = Power./x3; x1dot = -(u2/eta2+eta3*u3)/Ecap;x2dot = x3;x3dot=(Ft-Fres)./mass; output = [x1dot,x2dot,x3dot]; end function output = HaulTruckDynamics4(t,x,u,auxdata) eta1 = auxdata.eta1;eta2 = auxdata.eta2;eta3 = auxdata.eta3; Ecap = auxdata.Ecap;mu = auxdata.mu;g = auxdata.g;Cd = auxdata.cd; rho = auxdata.rho;A = auxdata.A;mass = 570e3;x3 = x(:,3); u1 = u(:,1);u2 = u(:,2);u3 = u(:,3);G = 0.1; Fr = mu*mass*g;Fg = G*mass*g;Fa = 0.5*Cd*rho*A*x3.^2; alpha = 1;beta = 1;gamma = 0;Fres = Fr + Fg + Fa; Power = alpha*eta1*u1+beta*eta2*u2+gamma*eta3*u3; Ft = Power./1;x1dot = -(u2/eta2+eta3*u3)/Ecap.*x3;x2dot = x3; x3dot = (Ft - Fres)./mass;output = [x1dot,x2dot,x3dot]; end function output = HaulTruckDynamics5(t,x,u,auxdata) eta1 = auxdata.eta1;eta2 = auxdata.eta2;eta3 = auxdata.eta3; Ecap = auxdata.Ecap;mu = auxdata.mu;g = auxdata.g;Cd = auxdata.cd; rho = auxdata.rho;A = auxdata.A;mass = 570e3;x3 = x(:,3);u1 = u(:,1); u2 = u(:,2);u3 = u(:,3);G = 0;Fr = mu*mass*g;Fg = G*mass*g; Fa = 0.5*Cd*rho*A*x3.^2;alpha = 1;beta = 1;gamma = 1; Fres = Fr + Fg + Fa;Power = alpha*eta1*u1+beta*eta2*u2+gamma*eta3*u3; Ft = Power./x3;x1dot = -(u2/eta2+eta3*u3)/Ecap;x2dot = x3; x3dot = (Ft - Fres)./mass;output = [x1dot,x2dot,x3dot]; end %************************* % END DYNAMICS FUNCTION %*************************

%************************* % START PATH OBJ FUNCTION %************************* function output = HaulTruckPathObj1(t,x,u,auxdata) 142

eta1 = auxdata.eta1;u1 = u(:,1);output = eta1*u1;end function output = HaulTruckPathObj2(t,x,u,auxdata) eta1 = auxdata.eta1;u1 = u(:,1);output = eta1*u1;end function output = HaulTruckPathObj3(t,x,u,auxdata) eta1 = auxdata.eta1;u1 = u(:,1);output = eta1*u1;end function output = HaulTruckPathObj4(t,x,u,auxdata) eta1 = auxdata.eta1;u1 = u(:,1);output = eta1*u1;end function output = HaulTruckPathObj5(t,x,u,auxdata) eta1 = auxdata.eta1;u1 = u(:,1);output = eta1*u1;end %************************* % END PATH OBJ FUNCTION %*************************

%************************* % START BND OBJ FUNCTION %************************* function output = HaulTruckBndObj1(t0,tf,x0,xf,auxdata) output = tf-t0;end function output = HaulTruckBndObj2(t0,tf,x0,xf,auxdata) output = tf-t0;end function output = HaulTruckBndObj3(t0,tf,x0,xf,auxdata) output = tf-t0;end function output = HaulTruckBndObj4(t0,tf,x0,xf,auxdata) output = tf-t0;end function output = HaulTruckBndObj5(t0,tf,x0,xf,auxdata) output = tf-t0;end %************************* % END BND OBJ FUNCTION %*************************

%************************* % START PATH CST FUNCTION %************************* function output = HaulTruckPathCst1(t,x,u,auxdata) eta1 = auxdata.eta1;eta2 = auxdata.eta2;eta3 = auxdata.eta3; mu = auxdata.mu;g = auxdata.g;Cd = auxdata.cd;rho = auxdata.rho; A = auxdata.A;mass = 250e3;x3 = x(:,3);u1 = u(:,1);u2 = u(:,2); u3 = u(:,3);G = 0;Fr = mu*mass*g;Fg = G*mass*g; Fa = 0.5*Cd*rho*A*x3.^2;alpha = 1;beta = 0;gamma = 0; Fres = Fr + Fg + Fa;Power = alpha*eta1*u1+beta*eta2*u2+gamma*eta3*u3; Ft = Power./x3;x3dot = (Ft - Fres)./mass;path1 = u1 - rimpull(x3); path2 = u3 - dynbrake(x3);path3 = x3dot;output = [path1,path2,path3]; end function output = HaulTruckPathCst2(t,x,u,auxdata) eta1 = auxdata.eta1;eta2 = auxdata.eta2;eta3 = auxdata.eta3; mu = auxdata.mu;g = auxdata.g;Cd = auxdata.cd; rho = auxdata.rho;A = auxdata.A;mass = 250e3;x3 = x(:,3); u1 = u(:,1);u2 = u(:,2);u3 = u(:,3);G = -0.1;Fr = mu*mass*g; Fg = G*mass*g;Fa = 0.5*Cd*rho*A*x3.^2;alpha = 0;beta = 0;gamma = 1; Fres=Fr+Fg+Fa;Power = alpha*eta1*u1+beta*eta2*u2+gamma*eta3*u3; Ft = Power./x3;x3dot = (Ft - Fres)./mass;path1 = u1 - rimpull(x3); path2 = u3 - dynbrake(x3);path3 = x3dot;output = [path1,path2,path3]; end function output = HaulTruckPathCst3(t,x,u,auxdata) eta1 = auxdata.eta1;eta2 = auxdata.eta2;eta3 = auxdata.eta3; mu = auxdata.mu;g = auxdata.g;Cd = auxdata.cd;rho = auxdata.rho; A = auxdata.A;mass = 250e3;x3 = x(:,3);u1 = u(:,1);u2 = u(:,2); u3 = u(:,3);G = 0;Fr = mu*mass*g;Fg = G*mass*g; Fa = 0.5*Cd*rho*A*x3.^2;alpha = 1;beta = 1;gamma = 1; Fres = Fr + Fg + Fa;Power = alpha*eta1*u1+beta*eta2*u2+gamma*eta3*u3; Ft = Power./x3;x3dot = (Ft - Fres)./mass;path1 = u1 - rimpull(x3); path2 = u3 - dynbrake(x3);path3 = x3dot;output = [path1,path2,path3]; end function output = HaulTruckPathCst4(t,x,u,auxdata) eta1 = auxdata.eta1;eta2 = auxdata.eta2;eta3 = auxdata.eta3; mu = auxdata.mu;g = auxdata.g;Cd = auxdata.cd;rho = auxdata.rho; A = auxdata.A;mass = 570e3;nu = 0.8075; x3 = x(:,3);u1 = u(:,1); u2 = u(:,2);u3 = u(:,3);G = 0.1; Fr = mu*mass*g;Fg = G*mass*g; Fa = 0.5*Cd*rho*A*x3.^2;alpha = 1;beta = 1;gamma = 0; Fres=Fr+Fg+Fa;Power = alpha*eta1*u1+beta*eta2*u2+gamma*eta3*u3; 143

Ft = Power./1;x3dot = (Ft - Fres)./mass; path1 = u1 - rimpull(x3)./ x3 * nu ;path2 = u3 - dynbrake(x3); path3 = x3dot; output = [path1,path2,path3];end function output = HaulTruckPathCst5(t,x,u,auxdata) eta1 = auxdata.eta1;eta2 = auxdata.eta2;eta3 = auxdata.eta3; mu = auxdata.mu;g = auxdata.g;Cd = auxdata.cd;rho = auxdata.rho; A = auxdata.A;mass = 570e3;x3 = x(:,3);u1 = u(:,1);u2 = u(:,2); u3 = u(:,3);G = 0;Fr = mu*mass*g;Fg = G*mass*g; Fa = 0.5*Cd*rho*A*x3.^2;alpha = 1;beta = 1;gamma = 1; Fres=Fr+Fg+Fa;Power = alpha*eta1*u1+beta*eta2*u2+gamma*eta3*u3; Ft = Power./x3;x3dot = (Ft - Fres)./mass;path1 = u1 - rimpull(x3); path2 = u3 - dynbrake(x3);path3 = x3dot; output = [path1,path2,path3];end %************************* % END PATH CST FUNCTION %*************************

%************************* % START BND CST FUNCTION %************************* function output = HaulTruckBndCst1(t0,tf,x0,xf,auxdata) output = [];end function output = HaulTruckBndCst2(t0,tf,x0,xf,auxdata) output = [];end function output = HaulTruckBndCst3(t0,tf,x0,xf,auxdata) output = [];end function output = HaulTruckBndCst4(t0,tf,x0,xf,auxdata) output = [];end function output = HaulTruckBndCst5(t0,tf,x0,xf,auxdata) output = [];end %************************* % END BND CST FUNCTION %*************************

144