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. iv 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 v 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 vi 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.