PathOptimizationUsingsub-RiemannianManifoldswithApplicationstoAstrodynamicsbyJamesKWhitingS.B.,AeronauticsandAstronautics,MassachusettsInstituteofTechnology(2002)S.B.,ElectricalEngineeringandComputerScience,MassachusettsInstituteofTechnology(2002)S.M.,AeronauticsandAstronautics,MassachusettsInstituteofTechnology(2004)SubmittedtotheDepartmentofAeronauticsandAstronauticsinpartialfulllmentoftherequirementsforthedegreeofDoctorofPhilosophyattheMASSACHUSETTSINSTITUTEOFTECHNOLOGYFebruary2011c JamesKWhiting,MMXI.Allrightsreserved.TheauthorherebygrantstoMITpermissiontoreproduceanddistributepubliclypaperandelectroniccopiesofthisthesisdocumentinwholeorinpart.Author . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .DepartmentofAeronauticsandAstronauticsJanuary18,2011Certiedby. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .Prof. OlivierdeWeckAssociateProfessorofAeronauticsandAstronauticsandEngineeringSystemsThesisSupervisorCertiedby. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .Prof. ManuelMartinez-SanchezProfessorofAeronauticsandAstronauticsThesisSupervisorCertiedby. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .Prof. RaySedwickAssisstantProfessorofAeronauticsandAstronautics,UniversityofMarylandThesisSupervisorAcceptedby . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .EytanH.ModianoChair,CommitteeonGraduateStudents2PathOptimizationUsingsub-RiemannianManifoldswithApplicationstoAstrodynamicsbyJamesKWhitingSubmitted to the Department of Aeronautics and Astronauticson January 18, 2011, in partial fulllment of therequirements for the degree ofDoctor of PhilosophyAbstractDierentialgeometryprovidesmechanismsforndingshortestpathsinmetricspaces. Thisworkdescribes a procedure for creating a metric space from a path optimization problem description sothattheformalismofdierential geometrycanbeappliedtondtheoptimal paths. Mostpathoptimization problems will generate a sub-Riemannian manifold. This work describes an algorithmwhich approximates a sub-Riemannian manifold as a Riemannian manifold using a penalty metricsothatRiemanniangeodesicsolverscanbeusedtondthesolutionstothepathoptimizationproblem. This new method for solving path optimization problems shows promise to be faster thanother methods, in part because it can easily run on parallel processing units. It also provides somegeometrical insights into path optimization problems which could provide a new way to categorizepathoptimizationproblems. Somesimplepathoptimizationproblemsaredescribedtoprovidean understandable example of how the method works and an application to astrodynamics is alsogiven.Thesis Supervisor: Prof. Olivier deWeckTitle: Associate Professor of Aeronautics and Astronautics and Engineering SystemsThesis Supervisor: Prof. Manuel Martinez-SanchezTitle: Professor of Aeronautics and AstronauticsThesis Supervisor: Prof. Ray SedwickTitle: Assisstant Professor of Aeronautics and Astronautics, University of Maryland34AcknowledgmentsI would like to thank everyone who made this research possible: my committee members - OlivierdeWeck, Manuel Martinez-Sanchez, andRaySedwick- forprovidingadviceasInavigatedthedepthsof dierential geometry; NASAand1.00forprovidingfundingasI workedthroughthemess of equations to create a small amount of order in the chaos of Newtonian gravity; my ocialreaders Thomas Lang and Paulo Lozano for feedback as I completed my research; and my unocialproofreadersDaleWinter,RachelandAlanFetters,andSarahRockwellforhelpingmetorealizewhere my explanations needed more clarication.I would like to thank my many friends for making life more enjoyable while I toiled on a seeminglyendless task of translating PhD level theoretical math into moderately usable engineering concepts.I would especially like to thank the Boston change ringers for providing a steady rhythm in my life,Tech Squares for reminding me to peel o from my research and shoot the stars every now and then,MetaphysicalPlantforremindingmethatallhuntsforknowledgeareeventuallycompleted, andMIT Hillel and TBS for the services and prayers that provided regular cycles in my life to mark thepassage of time. I would like to thank my high school cross-country team for training me to havethe endurance to keep going forever, my high school math club for encouraging me to study mathbeyond what I was taught, and the Cows for being good friends.I would like to thank my parents for everything they have done: providing a good home for metogrowupin,encouragingmetoexploretheabstractworldofideas,lettingmerushotoMITwhereIgotstuckforyearsinanendlessmazeofequationsthatImayhavenallyfoundapaththrough (suboptimal as it may have been), and most of all for teaching me the value of hard work,honesty, and persistence. I would like to thank my many siblings for living their lives so fully whileIwastoobusytodosoonmyown, providingmewithmanyniblingstoenjoy, andmakingtheholidays so full of love and excitement. I would like to thank my in-laws for providing some localfamily and support as Mira and I toiled through the challenges of graduate school. I would like tothank my extended family for being a good and supportive family, and Miras extended family forwelcoming me so quickly and lling our lives with love and happiness. I would also like to thank myson Jesse for making the nal months of this work much more exciting than they would have beenwithout him.And most of all I would like to thank my wife Mira, without whom I would never have made itall the way to the end. She has provided support and encouragement at every step of this process.I havelearnedhowtomovetheheavensandtheearthforherandI wouldliketoremindherthatsomethingsreallyarerocketscience. Melanyecceiluv`orenenyaoio, mobheancheileluinnarsilamiradilenya.56Contents1 Introduction 152 PreviousPathOptimizationMethods 172.1 Analytical Methods . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 182.1.1 Hamiltonian . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 182.1.2 Lagrangian . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 182.1.3 Dierential Inclusion. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 192.1.4 Kurush-Kuhn-Tucker Conditions . . . . . . . . . . . . . . . . . . . . . . . . . 192.2 Discretization Methods . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 202.2.1 Runge-Kutta Shooting. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 202.2.2 Finite Dierences . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 202.2.3 Pseudospectral Methods. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 203 AnIntroductiontoDierentialGeometry 213.1 Manifolds . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 213.1.1 Coordinates. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 223.2 Tensors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 223.2.1 Vectors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 223.2.2 Covectors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 233.2.3 General Tensors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 233.2.4 Summation convention . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 233.2.5 Tensor Fields . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 243.2.6 Flows . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 243.2.7 Wedge Product . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 243.2.8 Inner Product . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 243.3 Bundles . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 253.3.1 Bundles . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 253.3.2 Frames . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2573.3.3 Tangent Spaces. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 253.4 Parallel Transport and Connection . . . . . . . . . . . . . . . . . . . . . . . . . . . . 263.4.1 Connection . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 263.4.2 Parallel Transport . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 263.5 Derivatives . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 263.5.1 Partial Derivatives . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 273.5.2 Exterior Derivatives . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 273.5.3 Lie Brackets . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 273.5.4 Covariant Derivatives . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 283.6 Distributions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 283.6.1 Connection and Curvature . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 283.7 Measurements . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 293.7.1 Metric and Cometric. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 293.7.2 Metric Connection . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 303.8 Geodesics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 303.8.1 Geodesic Divergence . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 303.8.2 Geodesic Uniqueness . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 303.8.3 Singular Geodesics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 313.9 Branches of Geometry . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 313.9.1 Riemannian Geometry. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 313.9.2 Pseudo-Riemannian Geometry . . . . . . . . . . . . . . . . . . . . . . . . . . 323.9.3 Sub-Riemannian Geometry . . . . . . . . . . . . . . . . . . . . . . . . . . . . 323.10Formulas . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 343.10.1 Christoel Symbols . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 343.10.2 Riemann Curvature Tensor . . . . . . . . . . . . . . . . . . . . . . . . . . . . 343.10.3 Ricci Curvature Tensor . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 353.10.4 Sectional Curvature . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 354 PathOptimizationandsub-Riemannianmanifolds 374.1 Problem Formulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 374.1.1 Manifold . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 374.1.2 Moving Frame . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 384.1.3 Metric. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 394.1.4 Changing Frames. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 404.2 Optimal Paths . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 414.2.1 Uniqueness of Solutions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 414.3 Discussion. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4284.3.1 Hamiltonian Equivalence . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 424.3.2 3-D Example . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 445 SearchingforGeodesics 475.1 Finite Dierence Methods . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 475.1.1 Flat Space . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 475.1.2 Curved Riemannian Spaces . . . . . . . . . . . . . . . . . . . . . . . . . . . . 495.1.3 Computational Eciency . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 495.2 Pseudospectral Method . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 515.2.1 Computational Eciency . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 535.3 Stream Processing . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 536 HeisenbergManifolds 556.1 Denition . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 556.2 3-Dimensional Heisenberg Group . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 566.2.1 Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 586.3 Tank. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 606.3.1 Implementing the Algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . 646.3.2 Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 676.3.3 Curvature and Geodesic Uniqueness . . . . . . . . . . . . . . . . . . . . . . . 676.4 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 727 TheAstrodynamicsManifold 757.1 Coordinates and Frames . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 757.2 Metric and Cometric . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 797.3 Christoel Symbols . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 817.4 Lie Brackets . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 837.5 Connection, Curvature, and Torsion . . . . . . . . . . . . . . . . . . . . . . . . . . . 857.6 Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 868 ConclusionsandFutureWork 918.1 Comparison to other methods. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 918.2 Future Work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 92ASimulationCode 95A.1 The Heisenberg Manifold . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 95A.1.1 Finite Dierence Method . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 95A.1.2 Pseudospectral Method . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1009A.2 The Tank Manifold. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 104A.2.1 Pseudospectral Method . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 104A.2.2 Jacobi Fields . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 110A.2.3 Hamiltonian Integration. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 112A.2.4 Geodesic Integration. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 113A.3 Astrodynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 116A.3.1 Finite Dierence Method . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 116A.3.2 Pseudospectral Method . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 122A.3.3 Hamiltonian Integration. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 131A.4 Common Functions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13510ListofFigures4-1 The tank manifold . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 384-2 Diagramsshowinghowanoptimalpathcouldbefoundonacircleorsphere. Theupper left diagramshows theoptimal pathwithnoconstraints. As thepenaltyfunction is increased, the path moves towards the valid surface as shown in the upperright diagram. This method will not work if the path is completely orthogonal to theconstraints, because the projection operation will not work. . . . . . . . . . . . . . . 466-1 TheHeisenbergmanifoldpathoptimizationproblem. Findtheshortestpaththatencloses a xed area. The enclosed area goes from the origin to the initial point in astraight line, along the path to the nal point, and then in a straight line to the origin. 576-2 Movement of a single point in the path as the algorithm converges ( moves from .07towards 0). The horizontal line shows the analytically determined correct location ofthe point. The breaks in the line are from adding more points to the path. . . . . 596-3 Convergence rate of basic algorithm and extroplated movement algorithm. . . . . . 606-4 Convergencerateof theextrapolatedmovementalgorithmwithandwithoutnalpoint extrapolation. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 616-5 The tank manifold path optimization problem . . . . . . . . . . . . . . . . . . . . . . 626-6 Pseudo-code description of the algorithm . . . . . . . . . . . . . . . . . . . . . . . . . 656-7 Trajectory Variation as a function of. Higher values oflead to more movementin the x-direction and shallower steering angles. The lowest curve is the one closestto being vertical,the highestcurve is the one that moves farthest horizontally. values here are equally spaced logarithmically from 0.2 to 200. . . . . . . . . . . . . 686-8 log10 magnitude of the 2 Jacobi vector elds for a geodesic of the tank manifold with = 5 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 716-9 Angle between the 2 Jacobi vector elds for a geodesic of the tank manifold. . . . . 726-10ComparisonoftheoriginalgeodesicandthegeodesicfoundbycheckingtheJacobicondition . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 73116-11Close-up view of the geodesic found from the Jacobi condition, showing the very tightback and forth movements that allow the tank to move nearly sideways . . . . . . . 737-1 The astrodynamics manifold path optimization problem. This is for a coplanar orbitaltransfer using a two-body Newtonian gravitational model with point masses. . . . . 767-2 v as a function of dr for a sample trajectory. The v is increasing as the algorithmconverges, whiledrdecreasesuntil thepenaltyistoolarge, andthendrbeginstoincrease. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 887-3 vratio (geodesic divided by Hamiltonian) minus one. The graph shows the distri-bution of the error of the v calculation for the 216 generated paths. The table showsthe summary statistics of the distribution. The max and min are the ratio withoutsubtracting one. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8912ListofTables1 List of Symbols . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 142 Notation Directory . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1413p semi-latus rectumh modied angular momentum (_p/)execcentricity in the projectedx direction (e cos( + ))eyeccentricity in the projectedy direction (e sin( + ))f true anomaly longitude of the ascending node argument of perihelion longitude of perihelion ( +)L true longitude (f +) 1 +ex cos L +ey sinL = p/rThebasisdirectionsareradial, tangential, andnormal totheorbital plane. Thisiseectivelyaspherical coordinate system.Table 1: List of SymbolsWedge Product a b 3.2.7Inner Product < a, b > 3.2.8Vector ai3.2.1Covector ai3.2.2Summation Convention aibi3.2.4Exterior Derivative da 3.5.2Lie Bracket [x, y] 3.5.3Covariant Derivative a 3.5.4Christoel Symbols ijk3.10.1Thisworkdealsonlywithreal numbers. Whilemostof whatisdoneherewouldalsoapplytocomplex manifolds, the variablei is not meant to indicate the imaginary unit.Table 2: Notation Directory14Chapter1IntroductionPathoptimizationproblemshavebeenstudiedforover300years. Methodsofsolvingpathopti-mizationproblemshavetwoparts: ananalytical partthatderivestheoptimalityconditionsandanumerical partthatndsasolutiontotheoptimalityconditions. Thisworkfocusesonanewanalytical approach.A recent paper[1] described the various analytical approaches taken to solving path optimizationproblems. Manyanalyticalmethodsusethecalculusofvariationstoderivesomelocaloptimalitycondition. The most common example is adding a costate to create a Hamiltonian system.The current work is a fundamentally dierent approach to solving the path optimization prob-lem. Insteadof workingdirectlywiththeequationsof motionaspriormethodshavedone, thismethod is based on formulating the problem geometrically and then using the results of dierentialgeometrytodevelopananswer. Control problemshaveanatural geometricinterpretationbasedon using the state-space description of the problem to provide coordinates on a manifold. The costfunctions provide a metric for the manifold, which is then sub-Riemannian. The optimal paths arethe geodesics of the manifold.One advantage of using sub-Riemannian geometry to solve path optimization problems is thatthefeatures of themanifoldcanbestudiedtodiscover interestingproperties of theunderlyingproblem. Forexample, thecurvatureof themanifoldcanbeusedtodetermineif ageodesicisunique. If it is not unique, then there may be a dierent geodesic connecting the same points withashorterlength. Additionally, thereisaclassof curvescalledsingulargeodesicswhichdonotsatisfy the geodesic equation but are still length-minimizing curves. These solutions would never befound through calculus of variations based methods because they do not generally satisfy the localoptimality conditions.Previous methods of solving path optimization problems are summarized in chapter 2. This isfollowedbyabrief introductiontodierential geometryinchapter3, whichshouldbesucient15to allow most people with an undergraduate degree in engineering to understand the math in thiswork. Referencestomoredetailedmathtextsareincludedforreaderswhoareinterestedinamorethoroughdiscussionofthemathematicalconcepts. Thesetwochaptersprovidetherelevantbackground information for this research.Themostsignicantpartof thisworkischapter4, whichexplainstherelationshipbetweenpath optimization problems and geometrical manifolds. A method is presented which will producea sub-Riemannian manifold that corresponds to any path optimization problem. Another methodis presented to produce a reasonable penalty metric for the sub-Riemannian manifold that producesa compatible Riemannian manifold. Chapter 5 includes the other major part of this work, which istwo related methods for nding geodesics in Riemannian manifolds with penalty metrics.The next two chapters provide several detailed examples showing how the algorithms work. Chap-ter 6 includes an example based on the simplest possible sub-Riemannian manifold, the Heisenbergmanifold. It also has an example of working through all the calculations for the tank problem, witha detailed description of every step of the algorithm. Chapter 7 goes through the more complicatedproblemthatoriginallymotivatedthisresearch: coplanarorbitaltransfertrajectoryoptimization.The nal chapter summarizes the ndings of this research and provides some further research topicsthat have been opened by this work.16Chapter2PreviousPathOptimizationMethodsA path optimization problem can in general be stated as:Minimize the cost functionJ[x, u] =_tft0F(x(t), u(t), t)dt (2.1)subject to the dynamic constraints x(t) = f(x(t), u(t), t) (2.2)and the end point constraintsx(t0) = x0(2.3)x(tf) = xf(2.4)wherex(t) is the state-space description of the system andu(t) is the control vector of the system.x(t) andu(t) are both vector-valued functions of time (t).Therearetwostepstocreatinganalgorithmtosolvepathoptimizationproblems. First, ananalytical method has to be used to derive conditions that will determine when a path is optimal.Then a discretization method has to be used to convert the problem into one with a nite numberof points so that a computer can calculate a solution[1].172.1 AnalyticalMethods2.1.1 HamiltonianThe Hamiltonian formulation leads to a Hamiltonian function[2][3][4]H(x, u, , t) = F(x, u, t) +Tf(x, u, t) (2.5)whereFandfare the cost and constraints (as dened in section 2) and is the costate.The solution to the optimal path problem is then x =H(2.6) = Hx(2.7)Hu= 0 (2.8)When these equations can be satised, the solutions are optimal paths. For cases where the equationscannot besatised(normallytheresult of anoverconstrainedproblem), Pontryagins minimumprinciple provides the solution. Pontryagins principle states that the optimal solution is the feasiblesolution which minimizes the Hamiltonian.For ann dimensional optimal control problem, the Hamiltonian method adds ann dimensionalcostate, which doubles the dimensionality of the optimization problem fromn to 2n. However, theequations can be solved more easily, so the problem is often easier to solve even with twice as manydimensions.2.1.2 LagrangianThe Lagrangian formulation denes a Lagrangian functionL(x, x, , t) = F(x, x, t) +Tf(x, x, t) (2.9)where is the vector of Lagrange multipliers and varies with time.The solution is found by use of the Euler-Lagrange equationddt_L x_ =Lx(2.10)which will lead to the same equations as the Hamiltonian formulation. This method is slightly morerestrictive, because the controls do not appear in the Lagrangian, which means that this method canonly be used if it is possible to solve for the controls based on the trajectory. This will be the case18as long as the controls are linearly independent, so that the equation for x =f(u) can be invertedto produceu = g( x).The Lagrangian formulation also provides a way of nding singular solution paths. The singularpaths are the solution with the LagrangianL(x, x, , t) = Tf (2.11)Not all problems have singular solutions. A singular solution occurs when the constraints on aproblem are suciently restrictive that some paths have no local variations. These paths will not ingeneral satisfy local optimality conditions, but may still be part of an optimal path solution.This formulation will also lead to a TPBVP (two-point boundary value problem). However, thedimensionality does not necessarily double. The dimensionality of the problem goes from n to n+k,wherek is the number of constraints imposed on the dynamics.2.1.3 DierentialInclusionDierential inclusion is based on allowing functions to produce a range of values instead of a singlevalue. Ifthecontrolvariablescanbecomputedfromthechangesinstate,thenthecostfunctioncanbecalculatedfromthechangesinstate. Ratherthancalculatingoptimalityconditions, thedierential inclusion method derives formulas for the cost functions based only on the trajectory.[5]This formulation produces a non-linear programming (NLP) problem.2.1.4 Kurush-Kuhn-TuckerConditionsTheKKTconditionsaddanothersetofconstraintsforinequalitiesthatmustbesatisedbythesolution:g(t) 0 (2.12)where g(t) is also a vector-valued function of time. The formulation is then similar to the Lagrangianwith more constraints. The solution satises the following relations:F(x, x, t) +g(x, x, t) +f(x, x, t) = 0 (2.13)i 0 (2.14)

iigi = 0 (2.15)where is a vector of constants similar to Lagrange multipliers.192.2 DiscretizationMethods2.2.1 Runge-KuttaShootingA common method for solving a TPBVP is to integrate a path from one of the points until it eitherreachsthesecondpointoritisclearthatitwillnotreachthesecondpoint. Theconvergenceofshootingmethodsisrelativelyslow, becauseitisdiculttodeterminehowtomodifytheinitialtrajectory in a way that will modify the endpoint in a desired manner.2.2.2 FiniteDierencesAcontinuousdierential equationcanbeapproximatedbynitedierenceequationsatasetofdiscrete points. Finite dierence equations approximate the derivative of a function by the dierencesof the value of the function at nearby points. For example, dy/dx at point n could be approximatedby (yn+1 yn)/(xn+1 xn).2.2.3 PseudospectralMethodsA pseudospectral method uses a set of orthogonal functions to approximate a continuous function ata series of grid points. This can often provide better estimates of the derivatives of the function atthe grid points than nite dierence equations. Some common orthogonal functions include Fourierseries(sinesandcosines), Chebyshevpolynomials, andLegendrePolynomials. Forsomeofthesepolynomials, itmakesmoresensetohavenon-uniformspacingbetweenthediscretizationpoints.This allows the points to be located such that the orthogonal functions are easier to compute (forexample, nodes can be placed where many of the functions are equal to 0).20Chapter3AnIntroductiontoDierentialGeometryThis chapter provides a brief introduction to dierential geometry. Dierential geometry is the studyofgeometricalobjectsandhowtheychange. Therearemanytextsonthesubjectthatprovideamorecomprehensivedescriptionandderivationof theseconcepts andformulas. Spivaks seriesondierential geometry[6] providesathoroughexplanationof dierential geometry. DierentialGeometry for Physicists[7] is particularly good for people without a strong background in theoreticalmath. Thereareafewbooksonsub-Riemanniangeometry,includingonebyMontgomery[8]andanother by Calin and Chang[9]. The Riemannian geometry book by Manfredo Perdigao do Carmo[10]has a detailed discussion of the mathematics behind geodesics.3.1 ManifoldsA manifold is a set of connected points. Each point in a manifold is near some set of points (called theneighborhood of that point). The neighborhood of each point is similar to a Euclidean space. Thedimension of the manifold at each point is the dimension of the Euclidean space that it resembles.A smooth manifold has the same dimension at every point.Mathematical functions canbedenedwhichhavesomespeciedvalueat everypoint inamanifold. A smooth function does not vary much within a small neighborhood of any point in themanifold. A dierentiable function has continuous derivatives at all points in the manifold.Manifoldscanbedividedintosubmanifolds. Eachsubmanifoldhasall thepropertiesof theoriginalmanifoldexceptthatitdoesnotcontainallthepoints(butallthepointsitdoescontainmust be connected).213.1.1 CoordinatesCoordinates are a set of functions that taken together uniquely identify the points in a manifold. Theminimum number of functions required to do this for ann-dimensional manifold isn. A coordinatechart is a set of coordinate functions that span a portion of a manifold - meaning that they uniquelyidentify every point in that part of the manifold. An atlas is a set of charts that taken together spanthe entire manifold. Some manifolds can have a single chart that spans the entire manifold, whileother manifolds (such as a sphere) cannot.Any set of functions with the uniqueness property dene a valid coordinate system. This meansthattherearenonaturalorintrinsiccoordinatesforamanifold. Thecoordinateswill alsonot in general have any relationship to the geometrical properties of the manifold (other than thedimension).Coordinates are used to identify the points in a manifold. Without the coordinates, the manifoldstill exists in some abstract sense. The geometrical properties of the manifold are the same in everycoordinatesystem. Whatthecoordinatesprovideisameansof turninggeometryproblemsintoalgebraandcalculusproblems. Withcoordinatesonamanifold, thereissomewayofmeasuringhow close points are.3.2 Tensors3.2.1 VectorsA vector is a geometrical object with a length and a direction.Avectorspaceisasetofvectorsthatisclosedoveradditionandscalarmultiplication. Beingclosed over addition means that any two vectors in the vector space can be added and the result willalsobeinthevectorspace. Beingclosedoverscalarmultiplicationmeansthatanyvectorinthevectorspacecanhaveitslengthmultipliedbyascalartoproduceanothervectorwhichisalsointhe vector space. A vector space also has a linear operation for addition and scalar multiplication.All vectors in a vector space have the same dimension, which is also the dimension of the vectorspace. Each point in a manifold has a vector space attached to it, which has the same dimension asthe manifold at that point.When writing out the components of a vector, the indices are raised:v =

viei(3.1)where eiare the basis vectors.223.2.2 CovectorsA covector is a linear function from a vector space to the real numbers. One result of this denitionis that covectors also form a vector space, which is why they are also called dual vectors. A vector isalso a linear function from a covector space to the real numbers. An informal explanation of vectorsand covectors is that they function as row and column vectors (however that is not a mathematicallyprecise denition). An alternate name for a covector is a 1-form.When writing out the components of a covector, the indices are lowered:a =

aiei(3.2)whereiare the basis covectors.3.2.3 GeneralTensorsA tensor is a multi-linear geometrical object that is made of some number of vectors and covectors.A (k, l) tensor is a linear function that mapskvectors andlcovectors to the real numbers, whichalsomakesittheproductof kcovectorsandlvectors. Inparticular, a(1, 0)tensorisacovectorand a (0, 1) tensor is a vector. Vectors and covectors are combined through the tensor product tomake tensors. A (n, 0) tensor is also called ann-form.Tensors are geometrical objects, so they have an abstract geometrical existence that is indepen-dent of any coordinate systems. A tensor space can be dened at each point in a manifold for everytype of tensor. In general, the tensor space is only dened at a point, so tensors at dierent pointscannot be directly compared.Tensor components are written with their indices raised and lowered in accordance with whichindices correspond to vectors and which ones correspond to covectors.3.2.4 SummationconventionWhen writing out tensor formulas, it is common to want to sum the dierent components to producesomething like a dot product. For example, with the vectorv and the covectora, the producta(v)is written in components asa(v) = aivi=

iaivi(3.3)so the summation is implied and unwritten. Only repeated indices are summed, and only when oneis raised and the other is lowered.233.2.5 TensorFieldsA eld is a mapping of some type of tensor to every point of a manifold. A smooth eld has objectsthat vary smoothly in the neighborhood of every point in the manifold.A smooth vector eld is the solution to a dierential equation (it species a derivative at everypoint).3.2.6 FlowsThe ow of a vector eld is the set of paths that are produced by using the vector eld as a tangentvector at each point. If a uid were owing in the manifold with the velocity at each point given bythe vector eld, then the ow of the vector eld would be the ow of the uid.3.2.7 WedgeProductThe wedge product (denoted a b) is a mathematical operation dened on two tensors. The precisemathematical denition of the wedge product is fairly complicated, but it has properties similar tothe cross product. The wedge product is a linear antisymmetric operator, meaning that it has thefollowing properties:a b = b a (3.4)(a +b) c = (a c) + (b c) (3.5)a a = 0 (3.6)where a, b, and c are tensors. The wedge product of a k-form and an l-form is a (k +l)-form. Theseare the properties that matter for this work, and a more detailed description is available in any bookon dierential geometry.3.2.8 InnerProductTheinnerproduct(denoted)isamathematicaloperationdenedontwovectorsfromavector space with a metric that produces a real number. If the metric is the 2-formg, then theinner product is dened as:< a, b >= gab(3.7)The inner product is symmetric and linear in Riemannian and sub-Riemannian geometry.243.3 Bundles3.3.1 BundlesAbundleisamanifoldwithatensorspaceateverypointinthemanifold. Thetotal spaceofabundleisthesetofpointsinthemanifoldcombinedwiththevectorspaceateverypoint-inotherwordsthetotalspaceiseverypossibletensorateverypointinthemanifold. Abundlehasa projection operation which associates every point-tensor combination in the total space with thecorrect point in the manifold.A bundle is itself a manifold, where every point-tensor combination can be considered a point.A tensor eld is then a submanifold of the bundle, where every point has only one tensor associatedwith it.3.3.2 FramesFrames aresimilar tocoordinatesystems inthat theyprovidenumerical values for geometricalobjects. A frame is a set of linearly independent tensors that form a basis of a tensor space. Everytensorinthattensorspacecanthenbedescribeduniquelybythelinearcombinationofthebasistensorsthatisequal tothedesiredtensor. Thisallowstensorstobenumericallycomparedandmanipulated.A smooth frame eld is called a moving frame. A moving frame assigns a frame to every pointsuchthat nearby framesaresimilar. Typically onlythevector movingframeisspecied,becausethe covector frame can be determined from the vector frame (given a metric), and all other tensorframes can be determined from the vector and covector frames.Any smooth frame eld can be used as a moving frame. Any moving frame can be modied byrotations and stretching to produce another moving frame. Like coordinates, there is no naturalorintrinsicmovingframeonabundle. However, everycoordinatesystemproducesanaturalmoving frame by dierentiating the coordinate functions.A moving frame and coordinate system together produce a coordinate system on a bundle.3.3.3 TangentSpacesThetangentspaceofamanifoldisthevectorspaceofalltangentvectors. Atangentvectorisavector which points in a direction that is tangent to a feasible path in the manifold. For most typesof manifolds, thetangentspaceisof thesamedimensionasthemanifoldandfeasiblepathsrunthrough the point to all neighboring points. For sub-Riemannian manifolds, the tangent space is oflower dimension and there are some neighboring points that are not directly connected by a feasiblepathinthemanifold. Thetangentspaceateverypointformsavectorbundleonthemanifold,25called the tangent bundle.3.4 ParallelTransportandConnectionEverypointinamanifoldhasitsowntensorspaces(forvectors, covectors, andothertypesoftensors). The basis vectors and covectors are dened independently at each point in the manifold.Thismeansthatthereisnointrinsicwaytocomparetensorsatdierentpointsinthemanifold.The main topic of dierential geometry is how to connect the basis vectors and covectors at dierentpoints in a manifold in a geometrically reasonable way. Once the basis vectors and covectors can becompared at nearby points, it is possible to move tensors along a path and calculate how they arechanging in a geometrically meaningful way.3.4.1 ConnectionTheconnectionprovidesaone-to-onemappingoftensorsfromthetensorspaceatapointtothetensorspaceofanearbypoint. Twotensorsatdierentpointsinthemanifoldaregeometricallyidenticalifandonlyiftheconnectionsaystheyarethesame. Theconnectionprovidesawayofconnecting the tensor space at each point to the tensor space at nearby points.3.4.2 ParallelTransportParallel transport is a geometrical operation that moves a tensor along a path in a manifold whilekeepingitgeometricallyconstant. Itusesthelocaltransportoftheconnectiontomovefromonepoint to the next in the manifold. The connection is like a dierential equation that species howthings change locally, while the parallel transport is like an integration of that equation to go fromthe beginning of a path to the end.In general, parallel transport will be path dependent. The only exception is when the curvatureof the manifold is 0, in which case all paths connecting two points will provide the same mappingof vectors from the initial point to the nal point.3.5 DerivativesIn dierential geometry, dierent points in a manifold cannot be directly compared. In particular,thetangentspaceateachpointisauniquevectorspacethatisnotdirectlyrelatedtoanyothertangent space. This means that there is not in general any natural way to measure derivatives alonga vector or a path. Instead there are several reasonable ways to dene derivatives.263.5.1 PartialDerivativesA partial derivative can be calculated by ignoring the dierences between dierent tangent spacesand treating the space as Euclidean. This is mathematically useful as a component in calculatingotherderivatives, whichcanbedenedintermsof howtheymodifythepartial derivative. Thenotation for a partial derivative is the same as in regular calculus: x is the partial derivative in thex direction.3.5.2 ExteriorDerivativesThe exterior derivative operator only applies ton-forms. It converts ann-form to an (n + 1)-form,includingconvertingafunction(0-form)toa1-form. Theexteriorderivativeiswrittenasd. Forany forma,d(da) = 0.For a function f, dfis the standard dierential of the function - it is a linear operator on vectorsthat produces the directional derivative of the function in the direction of the vector, which is thegradient of the function.df(X) = f, X (3.8)where in this case, fis the gradient off, and X, Yis the dot product.For ann-forma = fdx1 dx2 . . . dxi = fdxI, the exterior derivative in coordinates isda =

fxidxi dxI(3.9)3.5.3 LieBracketsTheLiederivativeusesvectoreldstoequatedierentpointsinthemanifold. Acommonuseofthe Lie derivative is to calculate the Lie bracket of two vector elds. The Lie bracket measures thechange in a vector eld as it moves along the ow of a second vector eld. The notation for the Liebracket of vector eldsa andb is [a, b]. The Lie Bracket is anti-symmetric on its two arguments, so[a, b] = [b, a]. If two vector elds commute, then [a, b] = [b, a] = 0. The vector elds are only saidto commute when the Lie bracket is 0 everywhere, not just at some points.Physically, if two vector elds commute then movement along the two vector elds can happen inany order and the same point will be reached. This is the case in Euclidean coordinates. An exampleof non-commuting vector elds would bedx anddr as part of the standard coordinate vector eldsfor Cartesian and polar coordinates. Moving along the dx ow a distance of x and then along the drow a distancer does not in general lead to the same point as moving along thedr ow a distancerand then moving along thedx ow a distancex. The only exception is when the path starts onthex axis, in whichdr = dx.27Anysetofmovingframeswhereall pairsofbasisvectoreldscommutecanbeusedtoforma coordinate system by integrating the basis vectors from a chosen origin. A moving frame wheresome or all of the basis vector elds do not commute cannot be used to dene a coordinate system.In coordinates:[a, b]i= ajjbibjjai(3.10)3.5.4 CovariantDerivativesCovariant derivatives use parallel transport to make the tangent space at dierent points equivalent,which then allows derivatives to be calculated in a natural way. The covariant derivative is indicatedwith the operator: vis the covariant derivative in thev direction. In coordinates,(vu)i= vui+ ijkvjuk= vj uixj+ ijkvjuk(3.11)whereu andv are vectors. Ifu is instead a covector, then(vu)i = vui kijviuk = vj uixj kijvjuk(3.12)Higher order tensors have similar formulas, with the Christoel symbol term (ijk . . ., dened insection 3.10.1) subtracted for lower indices and added for rasied indices.3.6 DistributionsA distribution of rank k is a smooth k-dimensional subbundle of the tangent space on a manifold. Arankk distribution in ann-dimensional manifold can be dened by then k linearly independent1-formsiwith the property thati(X) = 0 for all vectorsXin the distribution. A distribution iscalled involutive if [X, Y ] is in the distribution for all vectorsXandYin the distribution. A sub-Riemannian manifold is a Riemannian manifold with the tangent space restricted to a non-involutivedistribution.3.6.1 ConnectionandCurvatureAdistribution hasacurvatureand connection that arebased on thegeometry of thedistributionindependent of any metric. These are based on the Carnot structure equations.For a rankk distribution spanned by the one forms1, ...k, the connection is the matrix of oneforms such thatdi =

jij j + ij(3.13)28where ijis the torsion matrix of 2-forms for the distribution. The distribution curvature isij = dij +ik kj(3.14)The distribution connection, torsion, and curvature determine how the distribution twists alongthe manifold that it is embedded in.3.7 MeasurementsWhile it is possible to solve some geometrical problems by using drawings, many problems are easiertosolveifthegeometricalobjectsaremeasuredsothattheproblemscanbesolvedusingalgebraandcalculus. Coordinatesandmovingframesprovidethemeasurements, butitisimportanttoremember that any chosen coordinate system or moving frame is not intrinsic to the manifold andthat the numerical results only have meaning when attached to the chosen coordinate system andmovingframe. Additionally, thegeometrical answerswill bethesameregardlessofthechoiceofcoordinates or moving frames.3.7.1 MetricandCometricAmetricdenestheinnerproductofavectorspace. Thatmeansthatametricisa(2,0)tensorbecause it is a function which takes two vectors and produces a real number (the value of their dotproduct). Since the metric is a geometrical object, the numerical values will depend on the framethat has been chosen, but the computed lengths of vectors will be the same in all frames.A cometric is the inverse of a metric. It is a (0,2) tensor which takes two covectors and producesa real number. In any given frame, the metric and cometric are the matrix inverses of each other.A metric on a manifold is a (2,0) tensor eld that provides a metric at every point in the manifold.A Riemannian metric is a smooth metric eld that is positive denite at all points in the manifold.A pseudo-Riemannian metric is similar except that instead of being positive denite, the metric isonly required to be non-degenerate. A positive denite metric has the property that every vector hasa positive length, which will be the case if the metric written out as a matrix is positive denite. Anon-degenerate metric is one in which there are no vectors other than the zero vector that have a dotproduct of zero with all other vectors. This will be the case if the metric has the same rank as thedimension of the manifold (meaning that all the rows of the metric matrix are linearly independent).Path lengths can be found by integrating the tangent vector along the path. Similar computa-tional methods can be used to nd areas, volumes, etc.293.7.2 MetricConnectionA metric connection is a connection that keeps the dot products of vectors constant as both vectorsare parallel transported along any path in the manifold. For any smooth metric on the manifold, itshould be possible to dene a metric connection.3.8 GeodesicsA geodesic is a geometrically straight line on a manifold. It is formed by moving along a tangentvectorandparallel transportingthetangentvectoralongthepath. Geodesicswill notgenerallyhaveasimpleequationliketheydoinEuclideanspaces. If thecurvatureof amanifoldiszeroeverywhere, thenthemanifoldis Euclideanandit is possibletouseCartesiancoordinates andmoving frames. Any manifold with non-zero curvature anywhere is non-Euclidean and will have atleast some geodesics that do not have a linear equation.A geodesic is the locally shortest path between two points in a manifold. It is possible that morethan one geodesic connects two points in a manifold, in which case only one of them is the globalminimum. Therearesomeconditionsthatcanbecheckedtoprovethatall geodesicsareglobalminimums.3.8.1 GeodesicDivergenceTwo nearby geodesics will follow dierent paths. The divergence between geodesics is described bythe Jacobi equation:D2dt2X = R(X, T)T (3.15)whereXis the vector describing the dierence between geodesics at lengtht, Tis the tangentvector to the geodesic at length t, and R is the Riemann curvature tensor (dened in section 3.10.2).This equation is only valid when the geodesics are still close enough that the Riemann tensor willnot be signicantly dierent on the two geodesics.3.8.2 GeodesicUniquenessInaRiemannianmanifold,anytwopointscanbeconnectedbyatleastonegeodesic. Sinceeachgeodesic is locally length minimizing, it is useful to determine when geodesics are unique and whenthey are not. If only one geodesic connects two points on a manifold, then it is the global minimumpath length between the points. If multiple geodesics connect two points, then one of them will bethe global minimum (although it is also possible to have multiple geodesics with the same length,for example the great circles on a sphere connecting antipodal points).30The simplest case where geodesics are unique is when the sectional curvature for every surfaceat every point is negative. That will lead to the Jacobi equation taking the form:D2dt2X = KX (3.16)whereKisapositivefunction. WhenXispositive, itwill becomemorepositive. WhenXisnegative, it will become more negative. This means that the divergence between geodesics will growas they spread out farther. If the sectional curvature is 0 everywhere then the second derivative is0 and the geodesics will diverge at a linear rate (this corresponds to a Euclidean space). In both ofthese cases, the geodesics will diverge from each other, so they will never intersect a second time.The only way for the geodesics to converge is if the sectional curvature is positive at some pointalongthegeodesic. Ifthishappens,thenitispossibleforsomegeodesicstoconvergeatmultiplepoints. An example of such a case is a sphere, which has a constant positive curvature. All the linesof longitude on the globe are geodesics that intersect at the poles.JacobiFieldsThe Jacobi equation can be used to generate Jacobi vector elds[10]. A Jacobi eld is a vector eldthat satisestheJacobiequation. TheJacobi eldscan beusedtodeterminewhen geodesicsareunique.ConjugatepointsonageodesicarepointswhereaJacobieldis0atbothpoints. Geodesicscan only intersect at multiple points if the points are conjugate to each other.3.8.3 SingularGeodesicsInsomesub-Riemannianmanifolds, itispossibletohaveonlyonepathconnectingtwopointsinthemanifold. Thispathisthentheminimumlengthpath, asnootherpathsexist. Sinceitisaminimal length path, it is a geodesic, but in general it will not satisfy the geodesic equation. Thesegeodesics are called singular geodesics[11].3.9 BranchesofGeometry3.9.1 RiemannianGeometryRiemanniangeometryisthemainbranchofdierential geometry; itisthestudyofRiemannianmanifolds. A Riemannian manifold has a smooth, positive-denite metric and a tangent space thathas the same dimension as the manifold. Riemannian manifolds have many useful properties thatmake them relatively easy to work with. In particular, every Riemannian manifold has a torsion-freemetric compatible connection, called the Levi-Civita connection.313.9.2 Pseudo-RiemannianGeometryApseudo-RiemannianmanifoldislikeaRiemannianmanifold, butthemetricisonlyrequiredtobenon-degenerateinsteadof positivedenite. Thespacetimeof General Relativityisapseudo-Riemannianmanifold. Becauseof theapplicationtoRelativity, thisisthemoststudiedareaofapplied dierential geometry3.9.3 Sub-RiemannianGeometryA sub-Riemannian manifold is like a Riemannian manifold, except the tangent space is of lower orequal dimension to the manifold. This means that Riemannian manifolds are actually a subset ofsub-Riemannian manifolds. In order for a manifold to be sub-Riemannian when it is not Riemannian,the tangent space has to change from point to point. The Lie brackets of the basis vectors for thetangentspacehavetoincludesomecomponentsthatareoutsideof thetangentspace. Thisisanequivalentconditiontotheproblemhavingnon-Holonomicconstraints. Thisisacomplicatedstatement which can best be described by an example.The state of a car can be described by its position (x,y), the angle the car is pointing in (), andthe angle of the steering wheel (). The car can move forward or backwards along a circle and canchange the radius of the circle, but cannot translate sideways or rotate in place. The total space isfour dimensional, but the tangent space is only two dimensional. All points in the space can still bereached through some path. The tangent space restriction makes some problems dicult to solve,such as parallel parking.When the car is pointing alongdx, = 0 d = a[d, dx] for some value ofa dy = b[d, dx] = ab[[d, dx], dx] for some values ofa,bVerticalandHorizontalVectorSpacesInasub-Riemannianmanifold, itispossibletopartitionthevectorspacesintoahorizontal partandavertical part. Thehorizontal partisthetangentspaceateachpoint. Thevertical partiseverything else. All vectors can be divided into their horizontal part and their vertical part, althoughone of these will be 0 for some vectors.CompatibleRiemannianManifoldsIn a sub-Riemannian manifold, lengths are only dened for horizontal vectors. The vertical vectorshave an undened length. It is possible to create a Riemannian manifold such that all the horizon-talvectorshavethesamelengthandalltheverticalvectorshaveadenedlength. Themanifold32can be further restricted by requiring all the vertical vectors to be orthogonal to all the horizontalvectors (which means their dot products are 0). A manifold with these properties is called a com-patible Riemannian manifold. Every sub-Riemannian manifold has an innite number of compatibleRiemannianmanifolds,becausethelengthsfortheverticalvectorscanbeanyarbitraryfunction.Theonlychangebetweenthesub-RiemannianmanifoldandthevariouscompatibleRiemannianmanifoldsisthemetric. EachcompatibleRiemannianmanifolddenesacompatibleRiemannianmetric.CreatingacompatibleRiemannianmanifoldallowsall theresultsofRiemanniangeometrytobeappliedtoamanifoldwhichissimilartothesub-Riemannianmanifold. Aparticularlyusefulfamily of compatible metrics is the penalty metrics, which have vertical vector lengths approachinginnity. Inthesub-Riemannianmetric, thevertical vectorshaveinnitelength. Thereforethepenalty metrics approach the sub-Riemannian metric as the penalty terms approach innity. Thisalso means that the geodesics in the penalty manifolds approach the sub-Riemannian geodesics.TheStepofaDistributionEvery sub-Riemannian manifold has a tangent distribution of some dimension n. A new distributioncan be formed by adding the Lie brackets of the tangent vectors to the original distribution, whichwill generallyincreasethedimensionalityof thedistribution. Thestepof thedistributionisthenumber of times that the Lie brackets have to be added to increase the dimension of the distributionall thewaytothedimensionof themanifold(plusonefortheoriginal vectors). ARiemannianmanifold has a step 1 distribution.For the car manifold, the distribution has a 2-dimensional tangent space (ds andd). The Liebracketisd=a[ds, d] forsomescalingconstanta. Thisbringsthetangentspaceuptothreedimensions(ds, d, andd). TheLiebracketonthesethreevectorsaddsthefourthdimensionthrough [ds, d].The growth vector of a distribution describes the number of dimensions spanned by the distri-bution plus the Lie brackets of the distribution. For the car problem, the growth vector is (2, 3, 4).Categoriesofsub-RiemannianManifoldsThesimplesttypeofsub-RiemannianmanifoldisaHeisenbergmanifold. AHeisenbergmanifoldhas the following properties:[9]The manifold is step 2 everywhereThe tangent space distribution is fully spanned bym orthonormal vector eldsXiThere are p = nm locally dened 1-forms with (Xi) = 0, which satisfy the nonvanishingconditions Det {a([Xi, Xj])}ij = 033where n is the total dimension of the manifold and m is the dimension of the tangent space (m < n).Oneconsequenceofthesepropertiesisthattherankofthedistribution(m)hastobeevenforamanifold to be a Heisenberg manifold. If the rank is odd, then the skew symmetric matrix dened inproperty 3 will always have a determinant of 0. Additionally, Heisenberg manifolds have no singulargeodesics.[9]It is worth noting that the denition of a Heisenberg manifold depends only on the tangent spaceof the manifold and is independent of the metric.A Grushin manifold is like a Riemannian manifold that has a metric which is singular at someplacesinthemanifold(forexample, ifitcontainsatermsuchas1/x2). AHormandermanifoldisasub-Riemannianmanifoldwhichisstep3orgreater. Moredetailsaboutdierenttypesofsub-Riemannian manifolds can be found in [9].3.10 Formulas3.10.1 ChristoelSymbolsThe Christoel symbols are used to measure how much the covariant derivatives dier from partialderivatives. The formula for calculating the Christoel symbols isijk =12gim_gjmxk+gmkxjgjkxm_(3.17)withgijas dened in section 3.2.8.3.10.2 RiemannCurvatureTensorTheRiemanncurvaturetensordescribeshowamanifoldisdierentlocallyfromaatmanifold.Geometrically, the Riemann tensor is dened asR(u, v)w = uvw vuw [u,v]w (3.18)The Riemann tensor measures the noncommutivity of the covariant derivative. If it is zero every-where, that means that the covariant derivative is commutative, so parallel transport is independentof path and depends only on the endpoints. This makes the space equivalent to a Euclidean space.The formula for calculating the components isRijkl=xkijl xlijk + ikmmjl ilmmjk(3.19)Rijkl=_2gilxjxk+2gjkxixl 2gikxjxl gjlxixk_+gmn_mjknil mjlnik_(3.20)34The Riemann tensor hasn4components, but it has the following symmetries:Rijkl = Rklij = Rjikl = Rijlk(3.21)In addition, the Bianchi identities further reduce the number of independent components:Rijkl +Riljk +Riklj= 0 (3.22)mRijkl +lRijmk +kRijlm= 0 (3.23)These identities leave onlyn2(n21)/12 independent components in the Riemann tensor. Thefollowing chart describes the number of independent components as a function of the dimension ofthe manifold.dimension components1 02 13 64 205 506 1053.10.3 RicciCurvatureTensorThe Ricci tensor is a contraction of the Riemann tensor.Rij = Rkikj = gklRikjl = gklRkilj =kijxkkikxj+ kijlkl liklikkjl(3.24)The Ricci tensor is also symmetric (Rij = Rji).3.10.4 SectionalCurvatureThe sectional curvature is the Gaussian curvature of a 2-dimensional submanifold within the man-ifold. Ifthesectionalcurvatureofallsurfacesatallpointsisnon-positive,theneverygeodesicisunique in the sense that only one curve satisfying the geodesic equation connects any two points onthe manifold. This condition means that geodesics are global optima instead of just local optima.It can be calculated asK(u, v) =R(u, v)v, uu, u v, v u, v2(3.25)whereu andv are vectors in the 2-dimensional submanifold.3536Chapter4PathOptimizationandsub-Riemannianmanifolds4.1 ProblemFormulationMany path optimization problems can be formulated using the language of sub-Riemannian geom-etry. Thisformulationprovidesaconvenientandstraightforwardmethodof ndingtheoptimalpath. A simple problem of a tank on a at surface will be used to illustrate the method. Any othervehiclethatcanrotateinplaceandmovefreelyalongaplanewithacostfunctionbasedonthetotal distance moved and the total angular rotation would be mathematically identical. Figure 4-1shows a diagram of the problem.4.1.1 ManifoldThemanifoldforapathoptimizationproblemisthesameasthestate-spaceformulationfortheproblem. Eachstatecorrespondstoonedimensionof themanifoldandprovidesonecoordinatefunction.For the tank, the state can be described using three variables. The position of the tank is given byits x and y coordinates and the current pointing direction is given by an angle . Other coordinatescouldbechosen, suchasusingpolarcoordinatesfortheposition. Theonlyrequirementforthecoordinates is that they must have a unique value (as a set) for every point in the manifold. It isuseful to choose coordinates that are natural and simple for the problem. For example,Cartesiancoordinates areoftensimpler thanpolar coordinates, but polar coordinates wouldgenerallybesimpler for rotational problems. Thebest coordinates tousewill dependonthenatureof theproblem.37!"#$ &"#'()*+ , -"./ 0123'4"2)# -5)6*73 !"# %" "&'()**+ (",- ) %)./ ,-012*- 3-%#--. &"1.%45 6789:7;7;9< 6= 89;9;9< > + ! ?)./ 2). (",- @"A#)AB ).B 3)2/ ?)./ 2). A"%)%- 1. &*)2- ?)./ 2).."% (",- 41B-#)+4 Figure 4-1: The tank manifold4.1.2 MovingFrameAmovingframefortheproblemhastobechoseninordertondthemetric. Themovingframeshould be chosen to produce a simple metric. This can be done by examining the cost function ofthe problem. For the tank problem, there is a cost for driving and a cost for turning. These are theonly feasible motions, so the tangent space is two dimensional.Whensolvingaproblemwherethetangentspaceisof lowerdimensionthanthetotal space,additional vectors have to be chosen to complete the moving frame. The additional vectors can beany vectors that fully span the vertical portion of the vector space. It is best to choose vectors thatare in some natural sense orthogonal to the feasible movements.For the tank, one more vector has to be chosen. A reasonable choice is moving the tank sideways.This is naturally orthogonal in the sense that there is no way to make these motions in any statethetankwill bein. Anotherpossibleoptionwouldbetranslationinthexdirection, butthisissometimes the same as driving the tank forward, which makes it a bad choice. The above choice ofa vector is also relatively simple so it will not complicate the math. Another possible option wouldbe rotating the tank about some xed point (which changes the pointing direction of the tank andits location), but this would lead to a more complicated metric.38Forthetank, theentiremovingframeisarepresentingthebasisvectorformovingthetankforward,b representing the basis vector for rotations in place, and c representing the basis vectorfor translating the tank sideways.4.1.3 MetricThesub-Riemannianmetricisgivenbythecostfunction. Thelengthsandanglesof vectorsinthe vertical sub-bundle are not well dened for a sub-Riemannian manifold, so they can be chosenarbitrarily. The simplest choice is to dene the basis vectors of the moving frame to be orthogonal tothe horizontal space. This choice will change the metric based on which vertical vectors are chosen,butinthelimitastheirlengthsaremadeinnite, all ofthesemetricswill convergetothesamesub-Riemannian metric. The lengths of the vertical basis vectors should depend on a parameter, sothey can be written as 1/ or something similar.Anyfunctionthatcanbedeterminedbyjustthepaththroughthestate-spacecanbeusedasa cost function. The metric is the derivative of the cost function, which makes the metric a linearfunction of the tangent vector at each point, but this does not impose any constraints on the costfunction. However, ifthecostfunctiondependsonthehistoryofthepathaswellasthecurrentstate,thecostfunctionwillbecomplicatedandthemethodmightnotworkwell. Anexampleofthis would be if the cost function for driving a car depends on the amount of fuel in the car, so thatdriving around a circle back to the same point would lead to a dierent local cost function. This canbe solved by adding all variables that the cost function depends on to the state-space description ofthe problem (which would mean adding the amount of fuel in the car as a variable in the descriptionof the cars state).Thecostfunctionforthetankisacostofonetomovethetankaunitofdistanceandacostofto rotate the tank one radian. The parameterrepresents the ratio of the cost of steering tothe cost of moving. A high value of will lead to solutions with little steering and lots of back andforthmovements,whilealowvalueof willleadtotightturnswithlittlemovement. Thesetwooperations are naturally orthogonal, so a b = 0. The lengths of the other basis vector will be setto 1/ with small. The metric is diagonal in this moving frame.Usingthecolumnvector =(a, b, c) torepresent thephysical motion, thecost functionisTg(). g() is the diagonal matrix with entries (1, 1/, ). So the cost function iscost = Tg() =_a b c___1 0 00 1/ 00 0 ____abc__(4.1)394.1.4 ChangingFramesInordertochangeframestothecoordinate-basedframe,therelationshipbetweenthecoordinatevectorsandthephysicalmovementvectorsmustbedetermined. Thisisdonebydeterminingthedierential equations that relate the movements to the state changes. For the tank,x = cos a sinc (4.2)y = sina + cos c (4.3) = b (4.4)In matrix form, this isX =__xy__ =__cos sin 0sin cos 00 0 1____abc__ = T (4.5)The inverse transformation is =__abc__ =__cos sin 0sin cos 00 0 1____xy__ = T1X (4.6)The metric in the new moving frame is derived from the old metric multiplied by the columnsof theinversetransformationmatrix. For example, xis thesamevector as cos a sinc, sogxx = cos2 + (1/) sin2. The other entries are all generated in the same way. In equations,cost = (T1X)Tg()T1X (4.7)= XT(T1)Tg()T1X (4.8)= XTg(X)X (4.9)g(X) = (T1)Tg()T1(4.10)g(X) =__cos2 + (1/) sin2 (1 1/) cos sin 0(1 1/) cos sin sin2 + (1/) cos2 00 0 __(4.11)40The cometric is the inverse of the metric and is produced in a similar way from the rows of thebasis transformation matrix:g(X)1= Tg()1TT(4.12)g1=__cos2 +sin2 (1 ) cos sin 0(1 ) cos sin sin2 +cos2 00 0 1/__(4.13)This is now a Riemannian metric on a Riemannian manifold.4.2 OptimalPathsThe optimal paths for the control problem correspond to the geodesics of the manifold. Any methodwhich will nd geodesics in a Riemannian manifold can be used to nd the optimal control trajec-tories. One method for nding the geodesics is given in chapter 5.4.2.1 UniquenessofSolutionsAs stated in section 3.8.2, the geodesics of a Riemannian manifold are unique if the sectional curva-ture of every two-dimensional submanifold is negative at every point. For many control problems,the resulting manifold is likely to have sectional curvatures that are sometimes positive and some-times negative. This will sometimes produce unique geodesics, but there may be some points thatare connected by multiple geodesics.Unless the sectional curvatures are always negative, Jacobi elds have to be used to determine ifthe geodesics are unique. This will not in general be possible to do analytically, but it can be donenumerically.Once a geodesic has been found that connects two points, Jacobi elds can be generated alongthe geodesic.1The initial value of all the Jacobi elds can be set to 0. Then the geodesic is unique ifthe Jacobi elds are all linearly independent along the geodesic. If the Jacobi elds are not linearlyindependent, then it is possible to create a Jacobi eld by adding some of the Jacobi elds togetherso that the new Jacobi eld is 0 at a second point.It is easiest togeneratetheJacobi elds byusingaslightlymodiedversionof theJacobiequation. Startingattheinitial pointwithanorthonormal basisthatincludesavectortangenttothegeodesic,thebasiscanbeparalleltransportedalongthegeodesic. Sinceparalleltransportpreserves length and angles, these vectors will remain orthonormal. Then there will ben 1 basis1Jacobieldsaredescribedinsection3.8.241vectors that are perpendicular to the geodesic (wheren is the dimension of the manifold). Each ofthese can be used to generate a Jacobi eld. Any other Jacobi eld will be generated by some linearcombination of thesen 1 elds. The rewritten Jacobi equation isJ(t) =

ifi(t)ei(t) (4.14)whereeiis the parallel transported basis vectors.Deningaij(t) = R(

(t), ei(t))

(t), ej(t) (4.15)leads to the dierential equationf

j (t) +

iaij(t)fi(t) = 0 (4.16)withtheinitialconditionsfi(0) = 0andf

i(0) = 1foroneofthefunctionsandf

i(0) = 0fortheothern 2functions. Thedierentialequationcanbenumericallyintegratedalongthegeodesicfor each of then 1 Jacobi vector elds to determine their values. If they are linearly independentatall pointsalongthegeodesic, thenthegeodesicisaglobal minimum. Iftheyarenotlinearlyindependentatsomepointalongthegeodesic,thentheyprovidethetangentvectorattheinitialpointwhichwill produceasecondgeodesicwhichshouldconvergewiththerstgeodesicattheconjugate point.[10]4.3 DiscussionIt is important to consider when this method is expected to work. Mathematically, this method isidentical to formulating the path optimization using a Hamiltonian method. Any control problemthat can be solved with a Hamiltonian method can also be solved by geodesic search.4.3.1 HamiltonianEquivalenceStarting with a manifold with a cometricgab, we can dene a functionH=12gabpapband call thisa Hamiltonian function. Using the standard notation from Hamiltonian mechanics, xa=Hpa= gabpb(4.17) pa = Hxa= 12gbcxa pbpc(4.18)42The second derivative ofx is xa= gabpb +gab pb(4.19)=gabxi xipb 12gabgcixb pcpi(4.20)=gabxi xipb 12gabgcjgilgcixb xj xl(4.21)= gajgbkgjkxi xigbl xl+ 12gabgjlxb xj xl(4.22)= gajgjkxi xi xk+ 12gabgjlxb xj xl(4.23)= gaigicxb xb xc+ 12gaigbcxi xb xc(4.24)= gai_gicxb+gibxc gbcxi_ xb xc(4.25)= abc xb xc(4.26)whichisthestandardRiemanniangeodesicequation[12]. Thisshowsthemathematicalequiv-alenceof thetwomethods, whicharebothbasedonndinggeodesicsinaRiemannianorsub-Riemannianmanifold. Thisalsoprovidesanalternatemethodfordeterminingthecometricofacontrol problem if the Hamiltonian solution has already been found.The Hamiltonian method of path optimization is based on creating a symplectic manifold whichcorresponds to the covector bundle of the base manifold. In other words, the Hamiltonian costateis the component representation of the tangent covector. The Hamiltonian function is a calculationof the tangent covector length, which is why the Hamiltonian function is constant along an optimalpath. Thepathonthemanifoldisfoundbyprojectingthecovectorontothetangentspaceandparallel transportingthetangentcovectoralongthepath. Thisproducesanoptimal trajectorybecause a geodesic is found by following a tangent vector as it is parallel transported along the pathit generates.In general, the sub-Riemannian cometric is singular, so it cannot be inverted to produce a metric.The cometric is used to convert a covector into a vector,which is why the costate can be used tond the tangent vector for the path. The metric is used to convert a vector into a covector, whichis why the derivatives of the state do not uniquely determine the costate. The rank of the cometricis the same as the dimension of the tangent space in the control problem. If the manifold dimensionisnandthetangentspacedimensionisk, thentherearen kmoredimensionsinthecovectorspace than in the vector space. In principle, it would be possible to use onlyn kcostates in theHamiltonian method because the otherk costates could be found from the derivatives of the state.43DierencesTherearetwomajordierencesbetweenthemethods. TherstisthattheHamiltonianmethodworkswithnomodicationsevenifthecometricissingular, whichmakesthemetricdegenerate.The second dierence is in how the geodesic equation is solved.The Hamiltonian method solves the geodesic equation by choosing a tangent covector and paralleltransporting it along a path. This will produce a geodesic, but there is no way to control where thegeodesic will go. The geodesic search method (described in chapter 5) will always produce a pathfrom the starting point to the ending point, but may not fully converge to a geodesic in a reasonableamount of time.In a Riemannian manifold, a tangent vector at a point fully denes a geodesic path. However,in a sub-Riemannian manifold, the tangent vector does not have enough information to fully specifyauniquegeodesic. Anyvertical vectorcanbeaddedtothetangentvectorwithoutchangingitsprojectionintothetangentspace. All ofthesedierentvectorswill appearthesameinthesub-Riemannianmanifold, butwillproducedierentgeodesicpaths. Itisthisfactwhichcomplicatesthe solution process for sub-Riemannian control problems.The Hamiltonian method solves this problem by using the tangent covector instead of the tangentvector. Since the cometric is singular but still nite,the tangent covector space is complete. Theprojectionofacovectorontothetangentspace(performedbythecometric)producesthepropertangent vector at each point. Parallel transporting the covector will then produce a geodesic.Thepenaltymetricmethodsolvestheproblembyallowingsmall deviationsfromtheactualtangentspace. Thesesmall deviationspermitthetangentvectorspacetobecomplete. Inthelimit as the penalty terms become innite, the tangent space collapses back to the sub-Riemanniantangent space.4.3.2 3-DExampleA representation of how this method (described in chapter 5) works can be demonstrated by con-sideringtheproblemofhowtondtheshortestpathbetween2pointsonasphere. Themethodpresented here is equivalent to drawing a straight line between the two points as an initial estimate ofthe path (this corresponds to having a very low penalty term). The path is easy to nd and optimalin some sense, but it does not actually satisfy the constraints of the problem. As the penalty termis increased, the path is penalized for failing to meet the constraint. The result of this is that thepath is projected onto the surface of the sphere, producing the geodesic of the original problem. Itis important to note that this projection operation is ambiguous when the path is connecting twoantipodal points on the sphere, because there is no particular direction to move the path in to bettermeet the constraints. The method presented here does not have a means to correct the projection44operation for cases where the non-constrained path is perpendicular to the tangent space.45Figure 4-2: Diagrams showing how an optimal path could be found on a circle or sphere. The upperleft diagram shows the optimal path with no constraints. As the penalty function is increased, thepath moves towards the valid surface as shown in the upper right diagram. This method will notwork if the path is completely orthogonal to the constraints, because the projection operation willnot work.46Chapter5SearchingforGeodesicsThe geodesic equation isd2xads2+ abcdxbdsdxcds= 0 (5.1)with the terms summed over all values ofb andc and the equation applied separately for eachvalue ofa (a,b, andc take on values corresponding to each dimension of the problem).In general this cannot be solved analytically. Previous methods of solving the geodesic equationinvolve discretizing the manifold.The algorithm developed here solves the geodesic equation directly by moving each point of thesolution until it is in the right place. The geodesic equation is modied to include an error termFad2xads2+ abcdxbdsdxcds= Fa(5.2)For ageodesic, theerror termwill be0. For anyother curve, theerror termwill provideinformation about how to modify the curve to make it closer to a geodesic.5.1 FiniteDierenceMethods5.1.1 FlatSpaceFor a Euclidean space, the modied geodesic equation isd2xads2= Fa(5.3)Consider the curvex = s2from (s, x) = (0, 0) to (s, x) = (1, 1). The derivatives aredxds= 2s (5.4)47d2xds2= 2 (5.5)which means thatFahas the constant value of 2. The geodesic with the same endpoints isx =s.For all points on the curve,s > s2, so the values ofx should be increased. Adding some multiple ofFaonto each point in the path will move the path closer to a geodesic. This is the general principleof this method.If the line is discretized into some number of points, the derivatives can be calculated numerically.Using a nite dierence method, the derivatives aredxds=xn+1 xn1sn+1 sn1(5.6)d2xds2=(xn+1 xn)/(sn+1 sn) (xn xn1)/(sn sn1)sn+1 sn1(5.7)with constant step sizes ins, this becomesdxds=xn+1 xn1s(5.8)d2xds2=xn+1 +xn1 2xn2(s)2(5.9)Putting this into the geodesic equation for at space,(xn+1 +xn1)/2 xn(2s)2= F (5.10)Theonly way thatFcan be0isif xn= (xn+1 + xn1)/2,which isthe expected equation fora straight line. WhenFis not 0, then the pointxnshould be moved. Using xnto represent theamount thatxnshould be moved for the next iteration of the method,xn =(xn+1 +xn1)/2 xnK(s)2(5.11)whereKis a stiness parameter that determines how much each point moves from one step to thenext. IfKs2= 1 then each point will be moved to the midpoint of its adjacent neighbors at eachstep. However, those neighbors will also have moved, so it will still take multiple iterations beforethealgorithmconverges. Ifthestinessistoohigh, thenthepointswillnotmovefarenoughateach step. If the stiness is too low, then the points will overshoot at each step and oscillate aroundthecorrectsolution. If Ks2< 1/2,thenthepointswillovershootbyalargeramountthanthecurrent error terms, which means the error terms will grow with each iteration of the algorithm.Having the correct stiness is critical to the performance of the algorithm. If the stiness is toohigh,thenthealgorithmwillconvergetooslowly. Whenrststartingthealgorithm,itisbestto48have a low stiness, so that the points can move quickly to somewhere near their correct locations.But once the algorithm has come close to converging, the stiness has to be increased so that thepoints will not overshoot or oscillate. The stiness should be adapted as the algorithm converges.Onewaytodothisiswithanoverrelaxtionmethod-increasingthestinesswheneverthepointovershoots,which can be determined by looking for a sign change in xnbetween iterations,anddecreasing the stiness whenever the point moves in the same direction for two iterations.5.1.2 CurvedRiemannianSpacesThe algorithm also works for curved Riemannian spaces. For an n-dimensional curve, the derivativesfor all coordinates have to be calculated. The calculations are still the same as before, but in curvedspace the Christoel symbols are generally not equal to 0.A simple curved space is the hyperbolic plane, which is a surface with constant negative curvature.The algorithm has been tested on this surface and nds geodesics with no diculties.5.1.3 ComputationalEciencyThenitedierencealgorithmconvergesfairlyquicklyforRiemannianspaces. Theconvergencerate is determined by how many points the path has and by how sti the path is (based on theKparameter). The stiness can be varied based on how the path is changing, which will automaticallytune it to the proper value for the manifold.If a high accuracy is desired for the path, then the answer will have to have a large number ofpoints. Adding points to the path increases the eective stiness, because more points have to bemoved and at each iteration the points will only move a small amount (because the points will beclose to their neighbors). The most computationally ecient way to nd paths with a large numberof points is to start with a small number of points and then add points as the path converges. Asan added benet, more points can be added to the parts of the path with more curvature, which iswhere a higher accuracy is needed most.Adding points can be done in two ways. One way is to add points to the entire path such thatthe new step size is constant, but smaller than the previous step size. The other way is to only addpoints to some part of the path, which will lead to a variable step size.The benet of adding points to only some parts of the path is that the grid can be left coarsewhere it does not aect the accuracy of the curve. The benet of adding points everywhere is thatit is computationally easier, so it might be faster even with more points.49VariableStepSizeAllowingavariablestepsizecanprovideamoreecientalgorithminsomecases. However, ifthestepsizesarenotconstant,thenthederivativesarealittlemorecomplicatedtocalculate. Aquadratic polynomial can be t to the three points, which will provide the derivatives. The followingsubstitutions are used in this derivation:X1= xn1 xn(5.12)X2= xn+1 xn(5.13)S1= sn1 sn(5.14)S2= sn+1 sn(5.15)The equations to be solved areaS21 +bS1= X1(5.16)aS22 +bS2= X2(5.17)b(S2 S22/S1) = X2 (S22/S21)X1(5.18)b =S21X2 S22X1S1S2(S1 S2)(5.19)aS21 +S21X2 S22X1S2(S1 S2)= X1(5.20)aS21=S21X2 +S22X1 +S1S2X1 S22X1S2(S1 S2)(5.21)aS21=S21X2 +S1S2X1S2(S1 S2)(5.22)aS21=S1S2S2X1 S1X2S1 S2(5.23)a =1S1S2S2X1 S1X2S1 S2(5.24)a =X1/S1 X2/S2S1 S2(5.25)a =X2/S2 X1/S1S2 S1(5.26)The derivatives aredxds= b (5.27)=S21X2 S22X1S1S2(S2 S1)(5.28)=(sn+1 sn)2(xn xn1) + (sn sn1)2(xn+1 xn)(sn sn1)(sn+1 sn)(sn+1 sn1)(5.29)50d2xds2= 2a (5.30)= 2(xn+1 xn)/(sn+1 sn) (xn xn1)/(sn sn1)sn+1 sn1(5.31)This modied algorithm has been tested with both at space and the hyperbolic plane. In bothcases it converges well to the same solution as with constant step sizes.AddingPointsWhenaddingpoints tothepath, themost useful places toaddmorepoints is totheparts ofthepathwherethepointwiseapproximationisleastaccurate. Thesewillbetheareaswherethesecond derivatives are changing most rapidly, because the path is eectively interpolating quadraticpolynomials between the points. Another useful place to add more points is where the manifold itselfhas a higher curvature. Higher curvature will lead to faster changes in the Christoel symbols, whichmeans that the approximation will be less accurate. Further work will be required to determine ifthere is a good way to determine where to add points so as to minimize the number of points needed.A much simpler algorithm is to just add a point between the two most separated points. Thiswill addpointstothepathsothattheyarealmostevenlyspaced, butrequiresusingalgorithmssimilar to what will be required for more adaptive point adding, so it is a good intermediate step.Asimulationwas runthat startedwithapathinthehyperbolicplanewithonly5points.Wheneverthetotalmovementofallthepointsinthepathwassmallenough, anotherpointwasadded. Thisledtoconvergencewith1000pointsinthenal pathinabout7500iterations. Bycomparison, starting with 1000 points requires about 900,000 iterations, (about 120 times as manyiterations). Using variable step sizes reduces the computational rate by about a factor of four, butit is still much faster because an approximate solution is found with a small number of points andthen the grid is made ner. However, it is even faster to double the number of points each time thesolution converges and maintain a constant step size for any given number of points.5.2 PseudospectralMethodPseudospectralmethodsarebasedonusingsomenumberoforthogonalfunctionstoapproximatethe curve. The functions are evaluated at the points on the path to determine where the path goesand what its derivatives are. Once the point locations and derivatives are computed, the algorithmis identical for calculating the virtual forces and point movements.There are several dierent sets of orthogonal polynomials that have been used for pseudospectralmethods. ThemostcommononesareLegendrepolynomials, Lagrangepolynomials, Chebyshevpolynomials, and Jacobi polynomials. For this research, the Chebyshev polynomials were used. TheChebyshev polynomials of the rst kind are dened as51T0(x) = 1 (5.32)T1(x) = x (5.33)Tn+1(x) = 2xTn(x) Tn1(x) (5.34)The Chebyshev polynomials of the second kind are dened asU0(x) = 1 (5.35)U1(x) = 2x (5.36)Un+1(x) = 2xUn(x) Un1(x) (5.37)The derivatives are:ddxTn(x) = nUn1(x) (5.38)d2dx2Tn(x) = n(n + 1)Tn(x) Un(x)x21(5.39)The path and its derivatives can be described as a sum of the rstm Chebyshev polynomials:f(x) =m

n=0anTn(x) (5.40)f

(x) =m

n=1nanUn1(x) (5.41)f

(x) =m

n=2nan(n + 1)Tn(x) Un(x)x21(5.42)This allows the rst and second derivatives to be computed algebraically once the path has beentbythepolynomials. If thecollocationpointsareused, thenthebestttothepathcanbecomputed by a process similar to a Fourier transform: the sum of the path function multiplied byeachpolynomialfunctionatallthepointsprovidesthecoecientofeachpolynomial(withsomecorrection to normalize the results). The collocation points are the solution of the equationx = cos(a/(m1)) (5.43)witha taking on the values from 0 tom1.The computational benet of the pseudospectral method is that it converts a calculus problem52intoalinear algebraproblem. Thecurrent point locationsaremultiplied by amatrix togeneratethe Chebyshev coecients, which are multiplied by another matrix to generate the rst and secondderivatives. These values are then combined through the geodesic equation to produce the virtualforcevectors,which providetheamount thateachpoint shouldbemoved ineachiteration ofthealgorithm (as described in section 5.1).Using pseudospectral methods to solve dierential equations with constant coecients convertsthedierential problemintoarelativelysimplelinear algebraproblemof addingandinvertingmatrices. However, the geodesic equation does not have constant coecients, because the Christoelsymbolschangedependingonwhatpointsthepathgoesthrough. Itmaybepossibletoperformsome kind of convolution procedure to multiply the Christoel symbol functions in the dierentialequation to produce a pure linear algebra problem. If this can be done, then the path optimizationproblem could be transformed into a small number of matrix operations.5.2.1 ComputationalEciencyAswithnitedierencemethods, thereisatradeobetweenaccuracyandspeed. Usingmorepoints(andthereforemoreorthogonal functions)requireslargermatricesandmorecomputation(roughlyO(n2)). However, using fewer points reduces the range of functions that can be accuratelyrepresentedbythepolynomials. Aswiththenitedierencemethods, themostecientwaytousepseudospectral methodsistostartwithasmall numberofpointsandthenaddmoreasthesolution converges. For most of the path optimization problems tried in this research, only the rstten coecients are signicantly dierent from zero, so eliminating the remaining polynomials wouldlead to only a tiny error. However,increasing the number of points still increases the accuracy ofthe solution, because the dierential equation is solved at more points along the path.5.3 StreamProcessingOnecomputational advantageofbothformsofthisalgorithmisthattheyareeasilyadaptedforstream processing. Stream processing is a form of vector processing that applies the same operationtomanydataitems. Manygraphicsprocessorscanimplementstreamprocessingwithasmanyas128parallel processingunits. Thisprovidesperformanceof upto1000billionoatingpointoperationspersecond(GFLOPs). Bycontrast, traditional CPUscanonlycomputeatarateofabout 25 GFLOPs per core, making the stream processors up to 40 times faster.Thealgorithms describedherearewell suitedtostreamprocessingbecausetheexact sameoperationsareperformedoneverymemberof thedataset(eachpointinthepath). Thenitedierence algorithm only requires information from three points for each points computations, whilethe pseudospectral method reduces to linear algebra operations that can be run on parallel execution53units easily. Both algorithms require computing the Christoel symbol values, which have no cross-dependencies between dierent points. Many other path optimization algorithms cannot be run fullyinparallel,becausetheyrequireinformationfromotherpartsofthepath. Forexample,shootingmethodscanonlyintegratefromeachpointtothenextpoint, sotheycannottakeadvantageofparallel execution capabilities.54Chapter6HeisenbergManifoldsHeisenbergmanifoldsarethesimplestandmostregularsub-Riemannianmanifolds. Thischapterprovides a description of two Heisenberg manifolds that the geodesic search algorithm can be appliedto. The rst example has an analytic solution, so the numerical solutions can be checked for accuracyand the convergence properties can be determined. The second example is a little more complicated,but the math is still simple enough that the equations can be written out fully and still be understood,soitprovidesagoodproblemforexplainingthealgorithminmoredetail. Thegeodesicsearchalgorithmworkswell forthesetwoexamples, anditislikelythatitwill workwell foranyotherHeisenberg manifold.6.1 DenitionHeisenberg manifolds are an important category of sub-Riemannian manifolds. Heisenberg manifoldslocally resemble the Heisenberg group, similar to how Riemannian manifolds are locally Euclidean.Asub-Riemannianmanifoldwithatangentspaceofdimensionmandatotalspaceofdimensionn = m+p is a Heisenberg manifold if it has the following properties:[9]1. There arem locally dened vector elds {Xi} such that the tangent space is fully spanned by{Xi}2. The vector elds ({Xi}) are orthonormal3. There arep 1 locally dened 1-formswith(Xi) = 0, which satisfy the nonvanishingconditions Det{([Xi, Xj])ij}= 04. Iflocalvectorelds {Xi}and {Yi}aredenedonlocalcharts, thenbothvectoreldsfullyspan the tangent space where the charts overlap55There are some other properties that can be derived from this denition. In particular, a Heisen-berg manifold is step 2 everywhere and the dimension of the tangent space has to be even.[9]One important consequence of the nonvanishing condition is that all Heisenberg manifolds satisfythe strong bracket generating condition.6.2 3-DimensionalHeisenbergGroupThe simplest Heisenberg manifold is the 3-dimensional Heisenberg group. This manifold has tangentvectorsa = x (y/2)t(6.1)b = y + (x/2)t(6.2)The Lie Bracket of these two vectors[a, b] = t(6.3)TheHeisenbergmanifolddescribesapathinthex, yplanewiththetcoordinaterepresentingthe area enclosed by the path (dened as _r2d in polar coordinates). The path length is equal tothe path length in thex, yplane. The geodesics are therefore the paths which enclose the desiredtotal area (specied by thet coordinate) with a minimum perimeter (see Figure 6-1).Applying the methodology of the previous chapters, we need to add a third vector to span thefull space of the manifold. c =tis a suitable choice because it is not in the tangent space and iscomputationallyeasytoworkwith. aandbbothhavealengthof1, whilechasalengthof (whichwillbeapproximatedas1/)becauseitrepresentsaphysicallyinfeasiblemovement. Thebasis change matrices are__abc__=__1 0 00 1 0y/2 x/2 1____xyt__(6.4)__xyt__=__1 0 00 1 0y/2 x/2 1____abc__(6.5)56!"#$"%&"'( *+%#,-./ 0 1+23 4567#8+6-% 1'-&."7 !"#$%&'% )*%" +&%,&&- %,# )#.-%' %"*% &-/0#'&' * 12&3 *$&*4 56726896: 5;72;89;: 8>: ?-/0#'&3 @$&* @ .' =.A&- Figure6-1: TheHeisenbergmanifoldpathoptimizationproblem. Findtheshortest paththatencloses a xed area. The enclosed area goes from the origin to the initial point in a straight line,along the path to the nal point, and then in a straight line to the origin.So the metric in the (x, y, t) system isg =__1 +y2/4 xy/4 y/2xy/4 1 +x2/4 x/2y/2 x/2 1/__(6.6)g1=__1 0 y/20 1 x/2y/2 x/2 (x2+y2)/4 +__(6.7)The corresponding connection coecients are:x=__0 y/4 0y/4 x/2 1/20 1/2 0__(6.8)57y=__y/2 x/4 1/2x/4 0 01/2 0 0__(6.9)t=__xy/4 (x2y2)/8 x/4(x2y2)/8 xy/4 y/4x/4 y/4 0__(6.10)The resulting geodesic equations are:d2xds2+y2dxdsdyds x2_dyds_2+1dydsdtds= 0 (6.11)d2yds2 y2_dxds_2+x2dxdsdyds 1dxdsdtds= 0 (6.12)d2tds2 xy4_dxds_2+x2y24dxdsdyds x2dxdsdtds +xy4_dyds_2y2dydsdtds= 0 (6.13)d2tds2 x2_y2dxds x2dyds +dtds_ dxds y2_y2dxds