Close
About
FAQ
Home
Collections
Login
USC Login
Register
0
Selected
Invert selection
Deselect all
Deselect all
Click here to refresh results
Click here to refresh results
USC
/
Digital Library
/
University of Southern California Dissertations and Theses
/
Discrete geometric motion control of autonomous vehicles
(USC Thesis Other)
Discrete geometric motion control of autonomous vehicles
PDF
Download
Share
Open document
Flip pages
Contact Us
Contact Us
Copy asset link
Request this asset
Transcript (if available)
Content
DISCRETE GEOMETRIC MOTION CONTROL OF AUTONOMOUS VEHICLES by Marin Kobilarov A Dissertation Presented to the FACULTY OF THE GRADUATE SCHOOL UNIVERSITY OF SOUTHERN CALIFORNIA In Partial Fulllment of the Requirements for the Degree DOCTOR OF PHILOSOPHY (COMPUTER SCIENCE) August 2008 Copyright 2008 Marin Kobilarov Acknowledgments This work would not have been possible were I not surrounded by many great people who have helped and encouraged me along the way. I would like to thank my family for their faith in me and constant support. I am deeply grateful to my adviser, Gaurav Sukhatme, who created an environment with unlimited academic possibilities that gave me complete freedom to explore interesting areas that I had not imagined before. Part of this environment were also the students, many of which good friends, at the USC robotics labs. I would especially like to thank Srikanth Saripalli and Boyoon Jung for their help during my rst year at USC. Towards the end of my doctoral work I had the unique chance to work closely with Mathieu Desbrun and Jerry Marsden at Caltech. I am grateful to Jerry and Mathieu for their guidance and source of knowledge that helped me shape this thesis and identify my future goals. I would like to thank Dr. David Ahlgren for introducing me to robotics in 1999. His dedication to learning was one of the reasons why I continued pursuing my dream and passion for robotics with the hope to contribute to science one day. ii Table of Contents Acknowledgments ii List Of Figures vi Abstract x Chapter 1: Introduction 1 1.1 Problem setup . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1 1.2 General approach and summary of results . . . . . . . . . . . . . . . . . . 4 1.3 Outline . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6 Chapter 2: A Discrete Geometric Framework for Optimal Control on Lie groups 8 2.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9 2.1.1 Related work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11 2.1.2 Contributions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12 2.2 Overview of Mechanical Integrators . . . . . . . . . . . . . . . . . . . . . . 14 2.2.1 Variational Integrators . . . . . . . . . . . . . . . . . . . . . . . . . 14 2.2.2 Lie Group Integrators . . . . . . . . . . . . . . . . . . . . . . . . . 16 2.3 Lagrange-d'Alembert-Pontryagin Principle . . . . . . . . . . . . . . . . . . 17 2.4 Direct Optimal Control Formulation . . . . . . . . . . . . . . . . . . . . . 23 2.4.1 Problem Formulation . . . . . . . . . . . . . . . . . . . . . . . . . 24 2.4.2 Algorithm Construction . . . . . . . . . . . . . . . . . . . . . . . . 25 2.5 Indirect Approach . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26 2.6 Computation on Matrix Groups . . . . . . . . . . . . . . . . . . . . . . . . 34 2.6.1 SO(3) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 34 2.6.2 SE(2) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 35 2.6.3 SE(3) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37 2.6.4 General matrix subgroups . . . . . . . . . . . . . . . . . . . . . . . 38 2.7 Underactuated Systems with Control Parameters . . . . . . . . . . . . . . 40 2.8 The sub-Riemannian case . . . . . . . . . . . . . . . . . . . . . . . . . . . 42 2.8.1 Discrete Equations of Motion . . . . . . . . . . . . . . . . . . . . . 44 2.8.2 Necessary Conditions for Optimality . . . . . . . . . . . . . . . . . 45 2.9 Eciency Techniques . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 46 2.9.1 Trajectory Initialization . . . . . . . . . . . . . . . . . . . . . . . . 47 iii 2.9.1.1 Geodesic Interpolation . . . . . . . . . . . . . . . . . . . 47 2.9.1.2 Lie Algebra Quadratic Interpolation . . . . . . . . . . . . 48 2.9.2 Trajectory Renement . . . . . . . . . . . . . . . . . . . . . . . . . 50 2.10 Applications . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 51 2.10.1 Rigid Body Reorientation on SO(3) . . . . . . . . . . . . . . . . . 51 2.10.2 Simple Helicopter in a Digital Terrain . . . . . . . . . . . . . . . . 52 2.10.3 Boat subject to External Disturbances . . . . . . . . . . . . . . . . 57 Chapter 3: Nonholonomic Integrators 61 3.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 62 3.2 Nonholonomic Systems with Symmetry . . . . . . . . . . . . . . . . . . . 65 3.2.1 Nonholonomic Connection . . . . . . . . . . . . . . . . . . . . . . . 68 3.2.2 Vertical and Horizontal Variations . . . . . . . . . . . . . . . . . . 69 3.2.3 Lagrange-D'Alembert-Pontryagin Nonholonomic Principle . . . . . 70 3.2.3.1 Vertical Equations . . . . . . . . . . . . . . . . . . . . . . 74 3.2.3.2 Horizontal Equations . . . . . . . . . . . . . . . . . . . . 74 3.3 Geometric Discretization of Nonholonomic Systems . . . . . . . . . . . . . 77 3.3.1 Discrete Approximation . . . . . . . . . . . . . . . . . . . . . . . . 77 3.3.2 Discrete Reduced LDAP Nonholonomic Principle . . . . . . . . . . 80 3.3.2.1 Vertical Equations . . . . . . . . . . . . . . . . . . . . . . 83 3.3.2.2 The Discrete Momentum Map . . . . . . . . . . . . . . . 83 3.3.2.3 Horizontal Equations . . . . . . . . . . . . . . . . . . . . 87 3.3.2.4 The case of linear connection . . . . . . . . . . . . . . . . 87 3.3.2.5 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . 90 3.4 Examples . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 91 3.4.1 Car with simple dynamics . . . . . . . . . . . . . . . . . . . . . . . 91 3.4.2 The Snakeboard . . . . . . . . . . . . . . . . . . . . . . . . . . . . 96 Chapter 4: Global Motion Planning 102 4.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 102 4.2 Sampling-based Roadmaps . . . . . . . . . . . . . . . . . . . . . . . . . . . 105 4.3 Planning using primitives . . . . . . . . . . . . . . . . . . . . . . . . . . . 110 4.3.1 Primitives . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 110 4.3.1.1 Trim Primitives . . . . . . . . . . . . . . . . . . . . . . . 112 4.3.1.2 Maneuvers . . . . . . . . . . . . . . . . . . . . . . . . . . 112 4.3.2 Computing motion plans . . . . . . . . . . . . . . . . . . . . . . . 113 4.4 Motion Planning using DMOC and Roadmaps . . . . . . . . . . . . . . . 114 4.4.1 Trajectory Discretization . . . . . . . . . . . . . . . . . . . . . . . 115 4.4.2 Sequencing Primitives . . . . . . . . . . . . . . . . . . . . . . . . . 117 4.4.3 Examples . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 119 4.5 Extensions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 123 4.5.1 Moving Goal State . . . . . . . . . . . . . . . . . . . . . . . . . . . 124 4.5.2 Optimal Coverage . . . . . . . . . . . . . . . . . . . . . . . . . . . 124 4.5.3 Uncertain Target Detection with Multiple Vehicles . . . . . . . . . 127 4.6 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 129 iv Chapter 5: Homotopy Continuation of Motion Planning Constraints 133 5.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 133 5.2 Homotopy Continuation . . . . . . . . . . . . . . . . . . . . . . . . . . . . 135 5.2.1 Embedding Method . . . . . . . . . . . . . . . . . . . . . . . . . . 136 5.2.2 Predictor-Corrector Method . . . . . . . . . . . . . . . . . . . . . . 137 5.3 Applications . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 139 5.3.1 Obstacle Constraints . . . . . . . . . . . . . . . . . . . . . . . . . . 139 5.3.1.1 Distance Function . . . . . . . . . . . . . . . . . . . . . . 140 5.3.1.2 Algebraic Obstacles . . . . . . . . . . . . . . . . . . . . . 141 5.3.1.3 Rough Terrain . . . . . . . . . . . . . . . . . . . . . . . . 143 5.3.2 Nonholonomic constraints . . . . . . . . . . . . . . . . . . . . . . . 147 5.4 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 149 Reference List 151 v List Of Figures 1.1 Vehicle operation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1 1.2 Segway RMP robot equipped with laser sensor, GPS, and omnidirectional camera (upper left). A typical scenario of following a fast moving per- son (upper right) and a view of the camera image of another scenario as the robot moves around the environment avoiding trees and tracking the correct person of interest . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2 1.3 Examples of developed vehicle models. . . . . . . . . . . . . . . . . . . . . 6 2.1 Successive renement of an optimal trajectory on SO(3) . . . . . . . . . . 51 2.2 Objective function convergence ratio vs. trajectory resolution. The graph shows average results from 3000 Monte Carlo simulations. . . . . . . . . . 51 2.3 Runtime vs. trajectory resolution (normal scale on left and log scale on the right). The graph shows average results from 3000 Monte Carlo simulations. 52 2.4 Simplied helicopter model used our tests. . . . . . . . . . . . . . . . . . . 53 2.5 Example of an optimized trajectory in a complex environment: a helicopter path through an outdoor canyon. . . . . . . . . . . . . . . . . . . . . . . . 54 2.6 Top and side views of the helicopter trajectory shown in Fig. 2.5 . . . . . 55 2.7 The orientation and position as functions of time for the helicopter trajec- tory shown in Fig. 2.5. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 55 2.8 The control forces and blade angles as functions of time for the helicopter trajectory shown in Fig. 2.5. . . . . . . . . . . . . . . . . . . . . . . . . . . 56 2.9 Planar boat controlled with two thrusters, and subject to hydrodynamic damping and wind forces. . . . . . . . . . . . . . . . . . . . . . . . . . . . 57 2.10 The USC RoboDuck2 boat used for experiments. . . . . . . . . . . . . . . 58 vi 2.11 Two optimal control scenarios with dierent boundary conditions and wind forces (shown as arrows along the path). The boat always starts at the ori- gin (;x;y) = (0; 0; 0) with zero velocity and must arrive at the designated positions with zero velocity. . . . . . . . . . . . . . . . . . . . . . . . . . . 60 3.1 Discrete approximation (dashed) of continuous trajectories (solid) in the shape space (left) using linear interpolation, and in the group (right) using local geodesics dened by the ow of the map . The discrete velocity vectors shown approximate the average velocity along the segment and satisfy the constraint as dened underneath the gures. These velocity vectors are attached at quadrature points determined by the choice of 2 [0; 1]. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 77 3.2 Evolution of the discrete momentum map. At point r k1 the map is com- puted by projecting the covector J loc k1 onto s r k1 dened by the basis e b (r k1 ); then in the Lie algebra basis attached at r k the covector J loc k1 transforms by Ad g 1 k g k+1 and the change in the momentum map is com- puted by subtracting it from the next momentum J loc k and projecting onto s r k (the notation J loc k := J loc (r k ;u k ; k ) was used with covectors drawn pointing towards the vectors that they act on). . . . . . . . . . . . . . . . 85 3.3 Car: pose & shape space variables. . . . . . . . . . . . . . . . . . . . . . . 91 3.4 Stability and eciency of our nonholonomic integrator for car trajectories: averaged over 50 runs using a large range of initial conditions and steering commands, our nonholonomic integrator remains as accurate as RK2, at a fraction of the computational complexity. . . . . . . . . . . . . . . . . . . 95 3.5 Snakeboard: pose & shape space variables. . . . . . . . . . . . . . . . . . . 96 3.6 Stability and eciency of our nonholonomic integrator for snakeboard tra- jectories: averaged over 50 runs using a large range of initial conditions and steering commands, our nonholonomic integrator remains as accurate as RK2, at a fraction of the computational complexity. . . . . . . . . . . . 101 4.1 A simple example: nding a trajectory from the start (lower-left corner) to a goal state inside the \bug-trap". A rapidly-exploring random tree (RRT) is used (left) to quickly explore the environment and nd any path (usually far from optimal). Once a path is found using RRT, the expansion switches to an incremental roadmap method (right) that focuses on nding a more optimal solution. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 105 4.2 Rapidly-exploring random tree (RRT) exploration pseudocode . . . . . . . 108 4.3 Incremental probabilistic roadmap (PRM) planning pseudocode . . . . . . 109 vii 4.4 Examples of optimal helicopter primitives (computed using DMOC) invari- ant with respect to the action of group G 0 = SO(2)R 3 . The top three trajectories are trim primitives of constant forward motions with transla- tional velocityv = 15m/s and rotational velocity! = 0 /s ( 1 );v = 15m/s and ! = 30 /s ( 2 ); v = 10m/s and ! = 15 /s ( 3 ). The bottom three trajectories are maneuvers that start from rest and achieve v = 15m/s and ! = 0 /s ( 1 ); start with v = 15m/s and ! = 0 /s and stop at rest ( 2 ); start from rest and achieve v = 15m/s and ! = 30 /s ( 3 ). Various concatenations of these primitives are then possible, e.g. 2 C 1 C 3 or 1 C 2 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 111 4.5 An example of planning using DMOC primitives and an incremental roadmap global search. A helicopter must traverse a cluttered urban environment and arrive inside the cylinder with minimum fuel. The dierent views show the resulting roadmap and least cost trajectory to goal after 1 second of computation. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 122 4.6 Incremental PRM for a car-like robot. The goal is to compute a trajectory that parks the car inside the -shaped parking structure. . . . . . . . . . 123 4.7 Finding a time optimal trajectory to reach a time-varying goal state with known dynamics. The goal state is the lower right at timet = 0 and starts to move north going around obstacles{its projected trajectory is drawn leaving the environment at time t = 100 s. Consecutive stages of the roadmap expansion show how the current best solution improves (the cost function is the time t f of reaching the goal ). . . . . . . . . . . . . . . . . 125 4.8 A vehicle that can sense everything outside the obstacles up to a xed radius. Maximizing the total area searched (shaded) in xed time horizon of of 100s. Consecutive stages of the roadmap expansion show how the current best solution improves (the coverage cost is the total shaded area shown). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 127 4.9 An example scenario: a target with uncertain dynamics is moving north avoiding obstacles. Its possible motion is represented by a nite set of particle trajectories. These trajectories are used to simulate future mea- surements taken by the vehicles. Two vehicles with circular sensing eld- of-view (with initial position in the lower left corner) are deployed in order to maximize the probability of detecting the target. . . . . . . . . . . . . . 128 4.10 A single target with time-varying uncertain dynamics (with distribution in time represented by particle trajectories) must be detection with maxi- mum probability and in minimum time. Two vehicles with circular sensing eld-of-view and limited velocity are deployed based on sampling-based roadmap computation. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 130 viii 4.11 Uncertainty in the helicopter position. Dierent motions resulting from uncertain external disturbance forces, e.g. arising from wind. The tra- jectories and the resulting position uncertainty ellipsoid are generated by sampling from a Gaussian external force error model. . . . . . . . . . . . . 131 4.12 A helicopter with uncertain dynamics (as shown in Fig. 4.11) must y to a goal location across a digital terrain. Its position estimate uncertainty norm is drawn as a tube along the path. It has no GPS and can only measure its own position by observing static beacons in the environment. The path on the left shows the shortest path to the goal but clearly demon- strates that the resulting uncertainty would increase the chance of collision with the terrain. The path on the right is aimed at minimizing the chance of collision by minimizing the position uncertainty along the path in the vicinity of obstacles. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 131 5.1 A car traveling in a tunnel. The initial trajectory is quickly computed by shrinking the obstacles to points. The obstacles are then grown and the trajectory repeatedly modied. . . . . . . . . . . . . . . . . . . . . . . . . 143 5.2 LittleDog manufactured by Boston Dynamics Inc.. The robot is used in the DARPA \Learning Locomotion" program with the goal to enable an unmanned vehicle to successfully traverse challenging terrains. One such terrain digitized from a real terrain patch is shown on Fig. 5.3. We use it as one of our test environments. . . . . . . . . . . . . . . . . . . . . . . . . 144 5.3 LittleDog on a digital terrain corresponding to a real laboratory terrain patch. The graphs show a few poses along an optimal path that successfully traverses the terrain. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 145 5.4 LittleDog's discrete trajectory on the terrain from Fig. 5.3. The terrain surface is initially smoothed and gradually deformed to its original. As the terrain changes the computed trajectory is adjusted accordingly to satisfy all stability and contact constraints. This type of \embedded" constrained optimization successfully yields an optimal solution, while optimization performed on the original terrain does not converge because of the high irregularity of the terrain. . . . . . . . . . . . . . . . . . . . . . . . . . . . 146 5.5 Homotopy stages of computing an optimal trajectory for a nonholonomic robot. Initially the robot is fully holonomic and a path can be computed very quickly. That path is then gradually deformed to account for the nonholonomic constraint as well as the optimality conditions. . . . . . . . 149 ix Abstract The goal of this work is to develop methods to optimally control autonomous robotic vehicles in natural environments. The main contribution is the derivation of state-space structure respecting integration and optimization schemes for mechanical systems with symmetries, controllable shape dynamics, and nonholonomic constraints based on the theory of discrete mechanics. At the core of this approach lies the discretization of variational principles of mechanics that results in various numerical benets previously unexplored in the area. The resulting framework is then used as a basis for developing optimal control methods applicable to various systems. Developed examples include sim- plied models of a car, a helicopter, a snakeboard, and a boat. The resulting algorithms are numerically stable, preserve the mechanical geometric structure, and are numerically competitive to existing methods. In addition, two important extensions with view to- wards practical applications are proposed. First, complex constraints are handled more robustly using homotopy continuation { the process of relaxing nontrivial motion con- straints arising either from complicated dynamics or from obstacles in the environment and then smoothly transforming the solution of such easier problem into the original one by deforming the constraints back to their original shape. Second, the optimality and x computational eciency of solution trajectories is addressed by combining discrete me- chanics and optimal control (DMOC ) with sampling-based roadmaps{a motion planning method focused on global exploration of the state-space. This allows the composition of simple locally optimal DMOC solution trajectories into near globally optimal motions that can handle complex, cluttered environments. xi Chapter 1 Introduction 1.1 Problem setup A vehicle is an actuated mechanical systems that moves, senses and performs a given task in the environment. The vehicle is equipped with sensors such as motor encoders, cameras, lasers or GPS. It uses these sensors to build a model of its surrounding, e.g. a map of the terrain, and to estimate its own state, e.g. its joint angles and its global position with respect to the environment. The following gure gives simplistic view of how a robotic vehicle operates. The robot moves around and senses the environment, then uses the sensor information to make decisions about the task to be executed, and then it controls itself in order to in order to complete the task. Consequently, its state Decision Making Sensing Control Figure 1.1: Vehicle operation 1 Figure 1.2: Segway RMP robot equipped with laser sensor, GPS, and omnidirectional camera (upper left). A typical scenario of following a fast moving person (upper right) and a view of the camera image of another scenario as the robot moves around the environment avoiding trees and tracking the correct person of interest 2 changes, it senses again, and so on. See Fig. 1.2 for an example of a vehicle performing person tracking and following outdoors. This thesis focuses on the control part of the vehicle operation. The main control framework developed in this work relies on two major assumptions: the vehicle has perfect knowledge of the environment and its state, the vehicle dynamical model is known in advance. Equivalently, it is assumed that accurate sensing is available and, and sensing uncertainty is not fully considered at the control stage. Nevertheless, sensing is extremely important and it is critical to account for the uncertainty in sensing and acting. Therefore, prelim- inary results in considering the eects of uncertainty at the control stage are included as an extension to our general framework. The goal of this work is to develop numerical methods to optimally control vehicles to achieve a given task. The task could be basic such moving to a goal state with minimum fuel or more complex such as nding a target by maximizing the area covered by the vehicle sensors in minimum time. In general, nding a solution to this types of problems is a dicult task. The main challenges lie in the fact that the vehicles can have complicated dynamics, are subject to constraints and underactuation, and must avoid colliding with complex obstacles. In contrast to local (or reactive) decision making and control, searching for an optimal solution requires the computation of a complete global trajectory from the vehicle current state to a desired state. This turns into the problem of searching among an innite number of possible paths. In practice, the trajectory of the vehicle is approximated using some 3 nite-dimensional representation or discretization. A major issue in this work is how to construct such a discretization in order to properly account for the underlying dynamics and to provide a numerically stable and ecient numerical framework. 1.2 General approach and summary of results Our general approach is based on a combination of standard optimal control techniques, and classical search and dynamic programming methods. These methods stand on top of a robust numerical representation of the underlying vehicle dynamics derived using the theory of discrete mechanics. The resulting formulation is general and applies to a variety of models widely used in robotics. In essence, this thesis develops a general control framework through a principled approach that is applicable to various type of vehicles and environments. While general optimal control frameworks exist, the proposed framework possesses benets previously unexplored in robotics. These can be summarizes as follows: structure-respecting geometric discretization of mechanical systems with symme- tries, internal actuated shape, and nonholonomic constraints discrete optimal control formulation that respects the geometric structure more robust constraint handling through homotopy-continuation techniques combining the derived local optimal control techniques with global search methods in order to guarantee near-globally optimal solutions 4 extending the basic motion planning framework to handle more specic tasks such as time-varying goal state, maximizing sensor coverage, deployment under goal motion uncertainty, planning for multiple vehicles The theoretical framework is implemented in software and applied to several problems. Some of the developed simple models and simulated scenarios include (see Fig. 1.3) a helicopter ying to a goal state in a digital terrain with minimum control eort (e.g., related to fuel consumption) a car moving optimally while avoiding other cars and static obstacles a boat performing optimal station keeping subject to currents a snakeboard generating optimal maneuvers computing a statically stable reference trajectory for the LittleDog robot computing optimal trajectories for cars and helicopters in cluttered urban environ- ment computing trajectories to moving targets with known dynamics computing a trajectory that maximizes the information gained about the uncertain position of a moving target in an environment with obstacles computing trajectory that maximizes the total area covered by a vehicle with a limited circular sensing radius among obstacles 5 (x,y) θ φ ψ torques damping damping thrusters wind (x,y) θ ψ φ φ Car Helicopter Boat Snakeboard Figure 1.3: Examples of developed vehicle models. Real Vehicles We must acknowledge the important fact that this thesis does not report any real-world experimental results. While this is unfortunate it does not mean that the proposed methods are not applicable to realistic applications. In fact, this whole work was motivated by the previous work of the author with real vehicles and the realization that a more unied, more optimal, and more principled numerical framework would be benecial for the proper development of autonomous control techniques. Hence, the algorithms developed here are aimed at and could soon be applied to useful robotics applications. Yet, one of the biggest issues that must be addressed before the proposed methods can be successfully used in practice the handling of uncertainty. Sensing uncertainty and model/actuation uncertainty are inherent in real vehicles and at the end of the thesis we would touch upon these issues and mention preliminary results and ongoing work that would enable the application of the proposed discrete geometric control framework on real systems. 1.3 Outline Ch. 2 deals with a simpler, restricted class of vehicles, i.e. systems with symmetries evolving on Lie groups, and develops their discretization and optimal control. Ch. 3 6 extends the discrete mechanical approach to systems with symmetries, shape variables, and constraints described in terms of a principle bundle and connection that geometri- cally encodes conservation laws and/or nonholonomic constraints. Ch. 4 proposes ways to extend DMOC for nding near globally optimal trajectories eciently by combining optimal control solutions with classical probabilistic motion planning techniques. Ch. 5 deals exclusively with methods that increase the robustness of control methods in dealing with complex constraints such as obstacles in the environment or mechanical constraints. We propose novel (in the context of motion control) homotopy-continuation methods that increase the eciency and radius of convergence of optimization-based methods through appropriate constraint deformation and embedding techniques. 7 Chapter 2 A Discrete Geometric Framework for Optimal Control on Lie groups Summary We consider the optimal control of mechanical systems on Lie groups that are left- invariant with respect to group actions. Our approach is based on the discretization of a Lagrange-d'Alembert-Pontryagin variational principle from which we derive structure- preserving discrete equations of motion. We rst apply a direct approach which uses the equations of motion as constraints in the optimization of a cost function (such as minimum control eort or minimum time) and results in a nonlinear programming problem. Then we propose a dierent formulation through the derivation of necessary conditions for optimality that results in a root-nding problem. In this indirect approach we use geometric methods to reduce the problem and transform it into numerically convenient form. Both approaches yield equivalent solutions but have dierent structure which motivates our numerical comparison. 8 We address the robustness and eciency of the optimization algorithms by proposing strategies for trajectory initialization and trajectory renement. The optimal control framework is demonstrated with several examples: rigid body minimum control eort reorientation, boat navigation subject to external disturbances, optimal trajectory computation of a simple helicopter in a digital terrain map. 2.1 Introduction We consider the optimal control of a mechanical system on a nite dimensional Lie group with Lagrangian that is left invariant under group actions. The goal is to move the system from its current state to a desired state in an optimal way, e.g. by minimizing distance, control eort, or time. The standard way to solve such problems is to rst derive the continuous equations of motion of the system. Among the trajectories satisfying these equations one can nd extremal (cost function minimizing) ones by solving a variational problem. Either direct methods using nonlinear programming or indirect methods based on the derivation of necessary conditions for optimality and root-nding are used to nd a solution. Both approaches require the discretization of the equations of motion into equality constraints suitable for numerical optimization. In contrast, we solve the optimal control problem by discretizing a variational principle, called Lagrange-d'Alembert-Pontryagin Principle. The principle yields a set of discrete trajectories that approximately satisfy the dynamics and that respect the state space structure. Among these trajectories we nd the ex- tremal ones without any further discretization. This allows important properties of the 9 mechanical system to be preserved and results in algorithms with provable accuracy and stability. We develop a general optimization framework for systems on Lie groups and apply it to the computation of optimal rigid body motions on SO(3), SE(2) and SE(3), as well as to general real matrix subgroups. In addition, we introduce techniques for trajectory initialization and renement that increase the algorithms performance. The Lagrangian Mechanical System Let the conguration space be a Lie groupG with algebra g and LagrangianL :TG!R that is left invariant under the action ofG. Using the invariance we can left-trivialize such systems by introducing the body-xed velocity 2 g dened by translation to the origin =TL g 1 _ g and the reduced Lagrangian` :TG=G!R such that`() =L(g 1 g;g 1 _ g) = L(e;). The Optimization problem The system is required to move from a xed initial state (g(0);(0)) to a xed nal state (g(T );(T )) during a time interval [0;T ] under the in uence of a body-xed control force 1 f(t)2g (i.e. an internal force produced by actuators in the body reference frame) while minimizing: J(g;;f) = Z T 0 C(g(t);(t);f(t))dt; (2.1) 1 In the Lagrangian setting a force is an element of the Lie algebra dual g , i.e. a one-formhf;i that pairs with velocity vectors to produce the total work R T 0 hf;idt done by the force along a path between g(0) and g(T ). 10 whereC :Ggg !R is a given cost function. For example, in case of minimum control eortC(g(t);(t);f(t)) = 1 2 kf(t)k 2 or for minimum time problemsC(g(t);(t);f(t)) = 1. 2.1.1 Related work Trajectory design and motion control of robotic systems have been studied from many dierent perspectives. Of particular interest are geometric approaches [31, 46, 51] that use symmetry and reduction techniques [43, 6]. Reduction by symmetry can be used to greatly simplify the optimal control problem and provide a framework to compute motions for general nonholonomic systems [47]. A related approach, applied to an underwater eel- like robot, involves nding approximate solutions using truncated basis of cyclic input functions [12]. There are a number of successful methods for motion planning with obstacles|see [35] for references. While standard optimization methods are based on shooting, multiple shooting, or collocation techniques, recent work on Discrete Mechanics and Optimal Control (DMOC, see [28, 30, 36]) proposes a dierent discretization strategy. At the core of DMOC is the use of variational integrators [41] that are derived from the discretization of variational principles such as Hamilton's principle for conservative systems or Lagrange-D'Alembert for dissipative systems. Unlike other existing variational approaches [47, 15] where the continuous equations of motion are enforced as constraints and subsequently discretized, DMOC rst discretizes the variational principles underlying the mechanical system dy- namics; the resulting discrete equations are then used as constraints along with a discrete cost function to form the control problem. Because the discrete equations of motion result from a discrete variational principle, momenta preservation and symplecticity are 11 automatically enforced, avoiding numerical issues (like numerical dissipation) that generic algorithms often possess. 2.1.2 Contributions In this chapter, we extend the generalized variational principle of [32, 5, 53] to the DMOC framework to derive optimization algorithms based on structure-preserving, discrete- mechanical integrators. In particular, we employ a discrete Lagrange-d'Alembert-Pontryagin principle to characterize mechanical systems with symmetries and external forces. We use this new discrete geometric optimal control framework for holonomic systems (possibly underactuated and/or with symmetries) and illustrate the implemented algorithms with a simulated example of a simplied helicopter ying through a canyon. The numerical benets of our discrete geometric approach are numerous. First, it automatically preserves motion invariants and geometric structures of the continuous system, exhibits good energy behavior, and respects the work-energy balance due to its variational nature. Such properties are often crucial for numerical accuracy and stability, in particular for holonomic systems such as underwater gliders traveling at low energies along ocean currents. Second, it benets from an exact reconstruction of curves in the Lie group conguration space from curves in its Lie algebra. Thus, numerical drift, for example associated with enforcing rotation frame orthogonality constraints, is avoided. Third, the simplicity of the variational principle allows exibility of implementation. Finally, this framework is exible enough to strike a balance between a desired order of accuracy and runtime eciency. 12 In addition to these well-documented advantages of discrete variational methods, there is growing evidence that DMOC methods are especially well suited for optimization prob- lems. In particular, their discrete variational nature seems to oer very good trajectory approximation even at low temporal resolutions. This stability vis-a-vis resolution is particularly suitable for design and exploration purposes as well as for hierarchical opti- mizations, as it leads to faster convergence towards optimal solutions. It is also important to note that non-holonomic constraints can also be imposed in our framework. While in this chapter we focus solely on holonomic systems with symmetries Ch. 3 contains initial results in the structure-preserving discretization of nonholonomic systems with symmetries and internal variables. The derivation of an indirect method for solving the optimal control for systems on Lie groups is another contribution described in this chapter. An indirect formulation commonly requires the use of additional state variables, i.e. Lagrange multipliers, that enforce the constraints. The main disadvantage of using an indirect method lies in the increased dimension of the problem, in the lack of systematic way to initialize these extra variables, and in increased problem sensitivity. These negative features reduce the radius of convergence and stability of the optimization problem. Our discrete geo- metric indirect formulation, on the other hand, possesses certain structure that permits the removal of the multipliers and the option to redene the problem in terms of its original optimization variables alone. This results in higher order optimality conditions with analogues form the continuous case [3] but with simpler and numerically convenient structure upon discretization. Standard nonlinear root-nding is then sucient to nd a 13 solution. Numerical evidence suggests that such a reduced problem does not suer from the aforementioned issues and exhibits faster and more stable convergence. 2.2 Overview of Mechanical Integrators A mechanical integrator integrates a dynamical system forward in time. The construc- tion of such numerical algorithms usually involves some form of discretization or Taylor expansion that results in either implicit or explicit equations to compute the next state in time. In an optimal control setting, these equations are then used as constraints. Instead, the integrators employed in this work are based on the discretization of vari- ational principles, i.e. variational integrators. In essence, they ensure the optimality (in the sense of Hamilton's principle, for instance) of the discrete path of the mechanical sys- tem in space-time. In addition, certain systems have group structure and symmetries that can be factored out directly in order to obtain more accurate and ecient integrators, e.g. Lie group integrators. After giving a brief overview of such integrators below we present a variational principle to derive more general integrators that account for symmetries. 2.2.1 Variational Integrators Variational integrators [41] are derived from a variational principle (e.g., Hamilton's prin- ciple) using a discrete Lagrangian. Unlike standard integration methods, variational inte- grators can preserve momenta, energy, and symplectic structure (i.e., a symplectic 2-form in phase space) for conservative systems; in the presence of forces and/or dissipation, they compute the change in these quantities with remarkable accuracy. Such features are ob- viously desirable for accurate dynamics simulation. The underlying theory has discrete 14 analogs of Noether's theorem and the Legendre transform, and a Lagrange-d'Alembert principle to handle non-conservative forces and constraints. Discrete mechanics, there- fore, stands as a self-contained theory similar to Hamiltonian or Lagrangian mechanics [5] and has already been applied to several domains: nonsmooth variational collision inte- gration [20], elasticity simulation in computer graphics [32], satellite formation trajectory design [29], optimal control of rigid bodies [36], of articulated bodies in uid [30, 50], and optimal control of wheeled robots [33]. In the variational integration setting, the state space TQ is replaced by a product of two manifolds QQ [41]. Thus, a velocity vector (q; _ q)2 TQ is represented by a pair of points (q 0 ;q 1 )2 QQ. A path q : [0;T ]! Q is replaced by a discrete path q d :fkhg N k=0 ! Q (q d =fq 0 ;:::;q N g, q k = q(kh)), Nh = T . One formulates a discrete version of Hamilton's principle (i.e. R T 0 L(q; _ q)dt = 0) by approximating the integral of the LagrangianL :TQ!R betweenq k andq k+1 by a discrete LagrangianL d :QQ!R L d (q k ;q k+1 ) Z (k+1)h kh L(q(t); _ q(t))dt: The discrete principle then requires that N1 X k=0 L d (q k ;q k+1 ) = 0; where variations are taken with respect to each position q k along the path, and the resulting equations of motion become D 2 L d (q k1 ;q k ) +D 1 L d (q k ;q k+1 ) = 0: 15 Example For example, consider a Lagrangian of the form L(q; _ q) = 1 2 _ q T M _ qV (q) and dene the discrete LagrangianL d (q k ;q k1 ) =hL q k+ 1 2 ; (q k+1 q k )=h , using the notationq k+ 1 2 := (q k +q k+1 )=2. The resulting equations are M q k+1 2q k +q k1 h 2 = 1 2 (OV (q k 1 2 ) +OV (q k+ 1 2 )); which is a discrete analog of Newton's law M q =OV (q). For controlled (i.e., non con- servative) systems, forces can be added using a discrete version of Lagrange-d'Alembert principle and discrete virtual work in a similar manner. 2.2.2 Lie Group Integrators Lie group integrators preserve symmetry and group structure for systems with motion invariants. Consider a system on conguration manifold Q = GM where G is a Lie group (with Lie algebra g) whose action leaves the system invariant, i.e., it preserves the induced momentum map. For example, G =SE(3) can represent the group of rigid body motions of a free- oating articulated body while M is a space of internal variables describing the joints of the body. The idea is to transform the system equations from the original state spaceTQ into equations on the reduced space gTM (elements ofTG are translated to the origin and expressed in the algebra g) which is a linear space where standard integration methods can be used. The inverse of this transformation is then used to map curves in the algebra variables back to the group. Two standards maps have been commonly used to achieve this transformation for any Lie group G: 16 Exponential map exp : g! G, dened by exp() = (1), with : R! G is the integral curve through the identity of the left invariant vector eld associated with 2g (hence, with _ (0) =); Canonical coordinates of the second kind ccsk : g ! G, ccsk() = exp( 1 e 1 ) exp( 2 e 2 )::: exp( n e n ), wherefe i g is the Lie algebra basis. A third choice, valid only for certain quadratic matrix groups [7] (which include the rigid motion groups SO(3), SE(2), and SE(3)), is the Cayley map cay : g! G, cay() = (e=2) 1 (e +=2): Although this last map provides only an approximation to the integral curve dened by exp; we include it as one of our choices since it is very easy to compute and thus results in a more ecient implementation. Other approaches are also possible, e.g., using retraction and other commutator-free methods; we will however limit our exposition to the three aforementioned maps in the formulation of the discrete reduced principle presented in the next section. 2.3 Lagrange-d'Alembert-Pontryagin Principle The Lagrange-d'Alembert-Pontryagin (LDAP) principle is a generalization of the Lagrange- d'Alembert variational principle and yields equivalent solution trajectories. The LDAP principle, however, provides additional freedom in the choice of variations which turns out to be crucial for obtaining symmetry and group structure preserving integrators and solving optimal control problems based on such integrators. 17 Continuous Reduced LDAP Principle Dene the reduced path (g;;) : [0;T ]!Ggg , and the control forcef : [0;T ]!g . The principle requires that Z T 0 `() +h;g 1 _ gi dt + Z T 0 h TL g 1 fg i dt = 0; (2.2) for variations g(t), (t), (t) that vanish at the endpoints. The curve (t) describes the velocity determined from the dynamics of the system. In view of the formulation, does not necessary correspond to the left-trivialized rate of change of the conguration g. The additional variables, though, indirectly enforce this dependence and correspond to both Lagrange multipliers and the momenta of the system. Thus, the principle generalizes Lagrange-d'Alembert principle and is linked to Pontryagin maximum principle of optimal control. After taking variations the continuous equations of motion become _ ad =f; =` 0 (); _ g =g (2.3) These equations 2 are called the Euler-Poincar e equations and denotes the system mo- mentum. 2 ad is dened byhad ;i =h; ad i, where ad = [;], 2 g. 18 Discrete Reduced LDAP Principle The discrete reduced Hamilton-Pontryagin principle for conservative systems was intro- duced in [5]. We make an elementary extension to these results for systems with internal forces. The discrete reduced path 3 is denoted by (g;;) 0:N :ft k g N k=0 !Ggg . Dene the discrete control force f 0:N :ft k g N k=0 ! g which approximates a continuous control force. The discrete reduced LDAP principle is formulated as: N1 X k=0 h `( k ) + k ; 1 (g 1 k g k+1 )=h k + N1 X k=0 TL g 1 k f k g k +TL g 1 k+1 f + k g k+1 = 0; (2.4) where the map : g!G relates Lie algebra elements to discrete changes 4 in the group conguration. is selected as a local dieomorphism such that ()() =e [5]. The left and right discrete forces f k 2g and f + k 2g (as shown below) are such that the work done by f along each discrete segment is approximated using the following quadrature (see [41] for more details): TL g 1 k f k g k +TL g 1 k+1 f + k g k+1 Z (k+1)h kh TL g(t) 1 f(t)g(t)dt 3 The term discrete path x0:N is also used to denote the set of all discrete congurations, i.e. x0:N = fx0;:::;xNg 4 Essentially, the choice of answers the question: How do we dene the dierence between two congurationsg k andg k+1 on the curved space, and based on that how do we compute the velocity along the segment between them. 19 The total work is therefore approximated by Z T 0 TL g(t) 1 f(t)g(t)dt N X k=0 TL g 1 k e f k g k ; (2.5) where e f 0:N :ft k g N k=0 !g is the resulting force at each discrete point and is dened by e f 0 :=f 0 ; e f k :=f + k1 +f k ; k = 1;::;N 1; e f N :=f + N : (2.6) After taking variations in (2.4) we obtain the following discrete equations of motion (see Sec. 4.2 in [5]). (d 1 h k ) k (d 1 h k1 ) k1 = e f k ; k = 1;:::;N 1; (2.7) k =` 0 ( k ); k = 0;:::;N 1; (2.8) g 1 k g k+1 =(h k ); k = 0;:::;N 1; (2.9) where d :g!g is the right-trivialized tangent of () dened by D() =TR () (d ); (2.10) and d 1 : g! g is its inverse 5 . Equations (2.7)-(2.9) can be considered as a discrete approximation to equations (2.3). 5 D is the standard derivative in the direction 20 In addition, using arguments similar to Sec.3.2.3 in [41] the discrete forced Noether theorem yields the following boundary conditions (d 1 h 0 ) 0 ` 0 ((0)) = e f 0 ; (2.11) ` 0 ((T )) (d 1 h N1 ) N1 = e f N ; (2.12) where (0) and (T ) are the initial and nal velocities. Note the distinction between 0 and(0) and N+1 and(T ). These quantities are not necessary the same since(t) refers to the point on the continuous curve at time t while k can be thought of as an average velocity along the k-th trajectory segment resulting from the discretization. The exact form of (2.7), (2.11), and (2.12) depends on the choice of. It is important to point out that this choice will in uence the computational eciency of the optimization framework when the equalities above are enforced as constraints. There are several choices commonly used for integration on Lie groups: the exponential map, canonical coordinates of the second kind (CCSK), and the Cayley map. In this work we employ the rst and the third types of methods (CCSK methods are based on the exponential map and can be considered closely related). 21 Exponential map The exponential map exp : g! G is dened by exp() = (1), where : R! G is a one-parameter subgroup of G such that _ (0) =. The right-trivialized derivative of the map exp and its inverse are dened as dexp x y = 1 X j=0 1 (j + 1)! ad j x y; dexp 1 x y = 1 X j=0 B j j! ad j x y; (2.13) where B j are the Bernoulli numbers. Typically, these expressions are truncated in order to achieve a desired order of accuracy. The rst few Bernoulli numbers are B 0 = 1, B 1 =1=2, B 2 = 1=6, B 3 = 0 (see [7, 24] for more details). Setting = exp, (2.7) becomes (dexp 1 h k ) k (dexp 1 h k1 ) k1 = e f k : Cayley map The Cayley map cay : g! R is dened by cay() = (I=2) 1 (I +=2). Based on this simple form, the derivative maps become 6 dcay x y = I x 2 1 y I + x 2 1 ; dcay 1 x y = I x 2 y I + x 2 : (2.14) Using = cay (see also [5]) (2.7) simplies to k k1 h 2 ad k k + ad k1 k1 h 2 4 k k k k1 k1 k1 = e f k : (2.15) 6 see Sec.IV.8.3 in [24] for derivation 22 The Cayley map provides only an approximation to the geodesic ow on the group (or to the exponential map). 2.4 Direct Optimal Control Formulation The most straightforward way to nd a numerical solution to the optimal control problem is to formulate a nonlinear program that minimizes the cost function over all discrete congurations, velocities, and forces, while satisfying the boundary conditions and the discrete equations of motion enforced as equality constraints. 23 2.4.1 Problem Formulation The optimal control problem for a system with reduced Lagrangian ` : g!R and xed initial and nal states (g(0);(0)) and (g(T );(T )) respectively can be directly formulated as Compute: 0:N1 ;f 0:N ;h minimizing J d (g 0:N ; 0:N1 ;f 0:N ;h) = N1 X k=0 C d (g k ; k ;f k ;h) subject to: 8 > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > < > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > : (d 1 h 0 ) 0 ` 0 ((0)) = e f 0 ; (d 1 h k ) k (d 1 h k1 ) k1 = e f k ; k = 1;:::;N 1; ` 0 ((T )) (d 1 h N1 ) N1 = e f N ; k =` 0 ( k ); g 0 =g(0); g k+1 =g k (h k ); k = 0;:::;N 1; 1 (g 1 N g(T )) = 0; k 2 [ l ; u ];f k 2 [f l ;f u ];h2 [h l ;h u ] (2.16) where C d is a discrete approximation of C dened in (2.1). The formulation allows time to vary and the last constraint places bounds on the time variation as well as bounds on all other variables. 24 Average velocity The variables denoted N and N have no eect on the trajectory g 0:N so we can treat these last points as irrelevant to the optimization. This is coherent with thinking of the velocities k as the average body-xed velocity along the k-th path segment between congurations g k and g k+1 . 2.4.2 Algorithm Construction Midpoint rule We choose the midpoint rule as a natural way to construct f andC d . According to the midpoint rule any discrete quantities dened over a trajectory segment are approximated at the segment midpoint. Therefore, the left and right discrete forces at each segment are equal: f k =f + k = h 2 1 2 (f k +f k+1 ); The discrete cost function then becomes C d (g k ; k ;f k ;h) =hC g k+ 1 2 ; k ; 1 2 (f k +f k+1 ) where g k+ 1 2 =g k ( h 2 k ), i.e. the midpoint along the ow dened by . Remark There are other choices besides the Midpoint rule that can lead to integrators of arbi- trary high order of accuracy, such as composition methods or symplectic Runga-Kutta 25 methods [41]. The midpoint rule is an appropriate choice for optimization problem since it provides a balance between accuracy and eciency. Implementation The optimal control formulation (2.16) can be solved using a standard constrained opti- mization technique such as sequential quadratic programming (SQP). One should choose a sparse SQP implementation to achieve scalability in the number of time steps. Sec. 2.10.1 provides empirical results and analysis of our implementation for an example rigid body control problem. 2.5 Indirect Approach In contrast to the direct approach, the optimal control problem can be solved by further analysis of solution trajectories through the derivation of conditions for optimality. An optimal solution is then the root of a system of nonlinear equations corresponding to these optimality conditions. In this section we derive necessary conditions for optimality for the constrained non- linear optimization problem (2.16) under some restrictions. The rst restriction is to assume that the reduced Lagrangian is of the form `() = 1 2 hI;i; (2.17) 26 where I : g! g is a symmetric positive denite linear map 7 . The second restriction is to specialize our derivation to systems with xed time-steph, and with minimum control eort cost function dened by J d (g 0:N ; 0:N1 ;f 0:N ) = N X k=0 1 2 k e f k k 2 ; (2.18) where e f k was dened in (2.6). Such cost function is physically meaningful since it cor- responds to the sum of the squared norm of the approximating force evaluated at each discrete point (see (2.5)). Since the evolution of the Lie algebra variables (2.7) is decoupled from the evolution on the group (2.9) we can remove the discrete group path g 0:N from the optimization state vector and express the boundary conditions g 0 = g(0) and g N = g(T ) in terms of 0:N1 . Dene the variables DEP 0 := (d 1 h 0 ) ` 0 ( 0 )` 0 ((0)); % initial momentum DEP k := (d 1 h k ) ` 0 ( k ) (d 1 h k1 ) ` 0 ( k1 ); % momentum equation DEP N :=` 0 ((T )) (d 1 h N1 ) ` 0 ( N1 ); % nal momentum REC := -1 (h 0 )(h N1 ) (g(0) 1 g(T )) 1 ; % reconstruction 7 h;i is the standard pairing between covectors and vectors of coordinates in the chosen Lie algebra basis 27 for k = 1;:::;N 1. The optimization problem is to compute 0:N1 and f 0:N that minimize (2.18) subject to DEP k = e f k ; k = 0;:::;N; REC = 0: (2.19) Clearly, the quantity DEP 0 is used to dene the initial velocity boundary condition, DEP{the discrete Euler-Poincar e equations or momentum balance equations along the path, DEP N {the nal velocity boundary condition, and REC{to dene the reconstruction equation. Proposition 1. The gradients of the constraints (2.19) are linearly independent. Proof. The proof follows directly from the fact that the DEP k constraints have full rank dependence on dierent variables for each k (i.e. each equation is a function of dierent pairs k , k+1 , and f k , f k+1 ) and that the maps (), d , and d 1 are non-singular for any . This proposition guarantees linear independence constraint qualication (LICQ) con- dition. Therefore, we can assume that necessary conditions for optimality do not include abnormal solutions (i.e. solutions that satisfy the constraints but do not account for the cost function). Furthermore, based on the above denitions, the cost function (2.18) can be re- expressed as J d (g 0:N ; 0:N1 ;f 0:N ) = 1 2 N X k=0 k DEP k k 2 ; 28 and since nowJ d is independent of the congurations and forces, the optimization problem can be stated as minimizing J d ( 0:N1 ) := J d (g 0:N ; 0:N1 ;f 0:N ) subject to REC = 0. Thus, the optimization can be performed with respect to the velocity variables alone. Dene the Lagrangian multipliers 2g and the Hamiltonian function H( 0:N1 ;) =J d ( 0:N1 ) +h; REC( 0:N1 )i (2.20) By Prop. (1) any optimal solution is normal and, therefore, must satisfy H = 0. Hence, we have the following necessary conditions for an optimal solution D k H k = 0; k = 0;:::;N 1; D H = 0; (2.21) for arbitrary variations k 2g,2g . These conditions are necessary but not sucient because, in general, the constraints are not ane and global optimality is not guaranteed. Further analysis of (2.21) requires the following lemmas. Lemma 1. The following relation holds for any ;;2g D Ad () = ad (d ()) Ad () Proof. Using the identity d () = Ad () d () (see [5]) we get D Ad () = (d ())()()() (d ())() = (d ()) Ad () ()()(d ()) = [d (); Ad () ] 29 Lemma 2. The dierential of the reconstruction operator REC along a path 0:N1 in the direction of variations k is D k REC ( 0:N1 ) k = d 1 (g) (h Ad (h 0 )(h k1 ) d h k ( k )); k = 1;:::;N 2; where g =(h 0 )(h N1 ) (g 1 i g f ) 1 Proof. Dierentiating D k REC ( 0:N1 ) k = D 1 (g)(h 0 )(h k1 ) d h k (h k )(h k )(h N1 ) (g 1 i g f ) 1 = d 1 1 (g) (h Ad (h 0 )(h k1 ) d h k ( k )) The following proposition is the main result in this section. Proposition 2. The trajectory of a discrete mechanical system on a Lie group G with algebra g and Lagrangian`( k ) = 1 2 hI k ; k i, with xed initial and nal congurations and velocities g(0)2 G, (0)2 g and g(T )2 G, (T )2 g, minimizes the total control eort P N k=0 1 2 k e f k k 2 only if the discrete body-xed velocity curve 0:N1 satises the following 30 Necessary Conditions for Optimality (d 1 h k ) k (d 1 h k1 ) k1 = 0; k = 1;:::;N 1 (2.22) 1 ((h 0 )(h N1 ) (g(0) 1 g(T )) 1 ) = 0; (2.23) where k 2g is dened by h k ;i = I(d 1 h k ( k ))h(d h k ) ad Ad (h k ) e f ] k+1 (d 1 h k ) I( k ); + D I( k );h D d 1 h k ( k ) E ; (2.24) k = Ad (h k ) e f ] k+1 e f ] k ; (2.25) e f 0 = (d 1 h 0 ) I( 0 )I((0)); (2.26) e f k = (d 1 h k ) I( k ) (d 1 h k1 ) I( k1 ); k = 1;:::;N 1 (2.27) e f N =I((T )) (d 1 h N1 ) I( N1 ); (2.28) Note: The proposition denesNn equations (2.22)-(2.23) in theNn unknowns 0 ;:::; N1 . A solution can be found using standard nonlinear root nding. Proof. The condition D k H = 0 for an arbitrary 2g is equivalent to D k D I( k ); d 1 h k (Ad (h k ) e f ] k+1 e f ] k ) E = D ; d 1 1 (g) (h Ad (h 0 )(h k1 ) d h k ()) E ; 31 for k = 0;:::;N 1 using the relation d 1 () = d 1 (Ad () ) [5] and Lemma (2). Setting k = Ad (h k ) e f ] k+1 e f ] k and applying the derivative on the left hand side in the above we get D I(d 1 h k (e k )); E + D I( k );h D d 1 h k ( k ) +h d 1 h k (ad (d h k ()) Ad (h k ) e f ] k+1 ) E = D h(d h k ) Ad (h 0 )(h k1 ) (d 1 1 (g) ) ; E ; (2.29) where have used the symmetry of I in the rst pairing and Lemma (1) in the second. Now dene the k-th reconstruction force k =h(d h k ) Ad (h 0 )(h k1 ) (d 1 1 (g) ) . Substituting it in (2.29) we nd the quantityh k ;i as dened by (2.24). The components of k can be directly computed using ( k ) =h k ;E i, wherefE g n =1 is the chosen Lie algebra basis. By the denition of k it is true that k = Ad (h k1 ) k1 and this relation is added as an optimality condition (2.22). The condition D H = 0 simply enforces the constraint (2.23). Geometric Interpretation We can think of the whole discrete trajectory as a single mechanical system consisting of a chain ofN +1 bodies, with the last body rigidly xed at congurationg(T ) and a force applied at congurationg(0) to keep the rst body xed there. If the rst body detaches from g(0) the instantaneous force that is pulling it back tog(0) is (d 1 1 (g) ) . This force has dierent representation on each of the remaining N bodies along the chain, i.e. through the Ad (h 0 )(h k ) transformation. The dynamics of the interaction between thek-th and (k+1)-th bodies is encoded by the velocities k1 , k , and k+1 and the reconstruction force k (which is simply the transformed ) and we 32 are able to express this force in terms of these three velocities through (2.24). When we do this for every segment along the chain we arrive at the relations (2.22) since it is the same force, just transformed along the chain. Thus, the original force is implicitly accounted for through the interactions of the bodies and need not be part of the optimization. Implementing the necessary conditions Implementation of equations (2.22)-(2.23) in Prop. (2) requires dening the maps, d 1 , andD d 1 . Two choices for discussed in Sec. 2.3 were the exponential map exp and the Cayley map cay and formulas for d 1 were provided in (2.13) and (2.14) respectively. D d 1 can be computed as (D dcay 1 0 )() = 1 2 ad 0 1 4 ( 0 + 0 ) (D d exp 1 0 )() = 1 2 ad 0 + 1 12 (ad 0 ad + ad ad 0) +:::; where the expression for Dd exp 1 can either be truncated to achieve a desired order of accuracy, or for groups such as rigid body motions can be redened in closed form. In the next section we provide more details about the algorithm implementation for the matrix groups SO(3), SE(2), SE(3) whose algebra can be identied with R 3 for the rst two groups andR 3 R 3 for the third. In these cases the operators Ad, ad, d 1 and D d 1 become matrices suitable for ecient implementation. 33 Comparison to the Continuous Case Necessary conditions for the continuous ana- log of the optimal control problem, i.e. minimizing R 1 2 kfk 2 dt subject to the Euler- Poincar e equations (2.3) and boundary conditions have been derived (e.g. [3]). For ex- ample, in the case of semi-simple compact groups the necessary conditions become D 2 @t 2 I D @t +R I D @t ; + 1 2 D @t ;I D @t + 1 2 I 1 I 2 D @t ; D @t = 0; (2.30) where D @t is the covariant time derivative and R is the curvature. The discrete version of these equations shows up as (2.22) in Prop. 2. Numerical discretization of (2.30), even in this simple case, would result into equations that would be no less complex than the ones in Prop. 2. Furthermore, our formulation remains unchanged for non-compact groups and, in fact, is readily applicable to general matrix subgroups as discussed below. 2.6 Computation on Matrix Groups 2.6.1 SO(3) Dene the mapb :R 3 !so(3) by b ! = 2 6 6 6 6 6 6 4 0 w 3 w 3 w 3 0 w 1 w 2 w 1 0 3 7 7 7 7 7 7 5 (2.31) A Lie algebra basis forSO(3) can be dened asfb e 1 ;b e 2 ;b e 3 g,b e i 2so(3) wherefe 1 ;e 2 ;e 3 g is the standard basis forR 3 . Elements2so(3) can be identied with the basis coordinates 34 !2 R 3 , i.e. = ! b e , or = b !. The commutator is identied with the cross product through [b !;b ] =!, 2R 3 . Using this identication we have cay(b !) = I 3 + 4 4 +k!k 2 b ! + b ! 2 2 : (2.32) The linear maps d , d 1 , and (D d 1 ) can be expressed as 3 3 matrices. In the Cayley map case these are dcay ! = 2 4 +k!k 2 (2I 3 +b !); dcay 1 ! = I 3 b ! 2 + !! T 4 (2.33) D dcay 1 ! = b 2 + ! T 4 + ! T 4 (2.34) 2.6.2 SE(2) The coordinates of SE(2) are (;x;y) with matrix representation g2SE(2) given by: g = 2 6 6 6 6 6 6 4 cos sin x sin cos y 0 0 1 3 7 7 7 7 7 7 5 : (2.35) Using the isomorphic mapb :R 3 !se(2) given by: b v = 2 6 6 6 6 6 6 4 0 v 1 v 2 v 1 0 v 3 0 0 0 3 7 7 7 7 7 7 5 for v = 0 B B B B B B @ v 1 v 2 v 3 1 C C C C C C A 2R 3 ; fb e 1 ;b e 2 ;b e 3 g can be used as a basis for se(2), wherefe 1 ;e 2 ;e 3 g is the standard basis ofR 3 . 35 The two maps :se(2)!SE(2) are given by exp(b v)= 8 > > > > > > > > > > > > > > > > > > > < > > > > > > > > > > > > > > > > > > > : 2 6 6 6 6 6 6 4 cosv 1 sinv 1 v 2 sinv 1 v 3 (1cosv 1 ) v 1 sinv 1 cosv 1 v 2 (1cosv 1 )+v 3 sinv 1 v 1 0 0 1 3 7 7 7 7 7 7 5 if v 1 6= 0 2 6 6 6 6 6 6 4 1 0 v 2 0 1 v 3 0 0 1 3 7 7 7 7 7 7 5 if v 1 = 0 cay(b v)= 2 6 6 6 6 6 6 6 4 1 4+(v 1 ) 2 2 6 6 4 (v 1 ) 2 4 4v 1 2v 1 v 3 + 4v 2 4v 1 (v 1 ) 2 4 2v 1 v 2 + 4v 3 3 7 7 5 0 0 1 3 7 7 7 7 7 7 7 5 The maps [d 1 ] can be expressed as the 3 3 matrices: [dexp 1 b v ] I 3 1 2 [ad v ] + 1 12 [ad v ] 2 ; (2.36) [dcay 1 b v ] = I 3 1 2 [ad v ] + 1 4 v 1 v 0 32 ; (2.37) where [ad v ] = 2 6 6 6 6 6 6 4 0 0 0 v 3 0 v 1 v 2 v 1 0 3 7 7 7 7 7 7 5 : 36 2.6.3 SE(3) We make the identication SE(3)SO(3)R 3 using elements R2SO(3) and x2R 3 through g = 2 6 6 4 R x 0 1 3 7 7 5 ; g 1 = 2 6 6 4 R T R T x 0 1 3 7 7 5 Elements of the Lie algebra 2 se(3) are identied with body-xed angular and linear velocities denoted !2R 3 and v2R 3 , respectively, through = 2 6 6 4 b ! v 0 0 3 7 7 5 ; where the mapb :R 3 !so(3) is dened in (2.31). Using this identication we have () = 2 6 6 4 (hb ! k ) h d h! k v k 0 1 3 7 7 5 ; where, choosing = cay, : so(3)! SO(3) is given by (2.32) and d ! : R 3 ! R 3 by (2.33). The matrix representation of the right-trivialized tangent inverses d 1 (!;v) :R 3 R 3 ! R 3 R 3 are computed as [dcay 1 (!;v) ] = 2 6 6 4 I 3 1 2 b ! + 1 4 !! T 0 3 1 2 I 3 1 2 b ! b v I 3 1 2 b ! 3 7 7 5 ; (2.38) 37 Note We should point out that the Cayley map yields a more computationally ecient integrator. In addition, it is suitable for iterative integration and optimization problems since the map and its derivatives do not have any singularities that might otherwise aect gradient based methods. 2.6.4 General matrix subgroups The Lie algebra of a matrix Lie group coincides with the one-parameter subgroup gen- erators of the group. Assume that we are given a k-dimensional lie subalgebra g gl(n;R). This subalgebra is isomorphic to the space of generators of a unique connected k-dimensional matrix subgroupGGL(n;R). Therefore, a subalgebra g determines the subgroup G in a one-to-one fashion ggl(n;R)()GGL(n;R): The two ingredients necessary to convert the necessary conditions in Prop. (2) into alge- braic equalities are: a choice of basis for g; and an appropriate choice of inner product (metric) that satises the invariance assumptions. Assume that the Lie algebra basis elements arefE g k =1 , E 2 g, i.e. that every element 2 g can be written as = E . The following inner product for any ;2 g would be useful hh;ii = tr(K T ); 38 where K 2 GL(n;R) is such thathhE ;E ii = and tr is the matrix trace. Corre- spondingly, a pairing between any 2g and 2g can be dened by h;i = tr(K); since the dual basis for g isf[E ] T g k =1 in matrix form. Example Ifg =so(3) with basis then settingK = diag(1=2; 1=2; 1=2) the pairing yields the standard inner product if we identify so(3) withR 3 , i.e.h;i = . Example If g = se(3) with basis then setting K = diag(1=2; 1=2; 1=2; 1) the pairing yields the standard inner product if we identify se(3) withR 3 R 3 . Kinetic Energy-Type Metric After having dened a metric pairing, a kinetic energy tensorI metric (such as the one used in 2.17) can be be expressed as hI();i = tr(KI d T ); where I d 2GL(n;R) is a symmetric matrix. Example Consider a rigid body on SO(3) with principal moments of inertia I 1 ;I 2 ;I 3 and Lagrangian`() = 1 2 I i 2 i where the i are the velocity components in the Lie algebra basis ?. The matrix I d must have the form I d = diag(I 1 +I 2 +I 3 ;I 2 +I 1 +I 3 ;I 3 +I 1 +I 2 ) 39 Example Consider a rigid body on SE(3) with principal moments of inertia I 1 ;I 2 ;I 3 , mass m, and Lagrangian `(!;v) = 1 2 I i ! 2 i +mv 2 i , where (!;v)2 se(3) are the body- xed angular and linear velocities and their components (v i ;w i ) are with respect to the Lie algebra basis ?. The Lagrangian in this case can be equivalently expressed as `() = 1 2 tr(KI d T ), where 2se(3) and I d = diag(I 1 +I 2 +I 3 ;I 2 +I 1 +I 3 ;I 3 +I 1 +I 2 ;m): 2.7 Underactuated Systems with Control Parameters The direct and indirect methods for mechanical systems on Lie groups can be generalized to underactuated systems. Assume that the control forces are applied along body-xed directions dened by the control covectorsff 1 ();:::;f c ()g, c n, f i : M! g which depend on control parameters : [0;T ]!M, whereMR m is the control parameter set. One can think of these extra parameters as the shape variables of the control basis, i.e. parameters that do not aect the inertial properties of the systems but which determine the control directions. Assume that the system is controlled using control input u : [0;T ]!U, whereUR c , is the set of controls applied with respect to the basisff i ()g. In addition, assume that the system is subject to position-dependent external force f ext : G!g . This general denition accounts for various types of forces. For instance, forces arising from a potential V :G!R take the form f ext (g) =TL g DV (g). 40 The discrete equations of motion of such system are (d 1 h k ) I k (d 1 h k1 ) I k1 h c X i=0 u i k f i ( k )hf ext (g k ) = 0 (2.39) Dene the Lagrangian multipliers 2g , k 2g and the Hamiltonian function H( 0:N1 ;u 0:N ; 0:N ; 0:N ;) = N X k=0 h 2 u T k u k +hDEP k h c X i=0 u i k f i ( k )hf ext (g k ); k i ! +h; REC( 0:N1 )i; where DEP is dened in Sec. 2.5. The necessary conditions for an optimal trajectory are dened in the following propo- sition (which extends Prop. 2). Proposition 3. The trajectory of a discrete mechanical system on a Lie group G with algebra g and Lagrangian`( k ) = 1 2 hI k ; k i, with xed initial and nal congurations and velocities g(0)2 G, (0)2 g and g(T )2 G, (T )2 g, minimizes the total control eort P N k=0 h 2 u T k u k only if the discrete body-xed velocity curve 0:N1 and control parameters 0:N satises the following 41 Necessary Conditions for Optimality (d 1 h k ) k (d 1 h k1 ) k1 = 0; k = 1;:::;N 1 (2.40) 1 ((h 0 )(h N1 ) (g(0) 1 g(T )) 1 ) = 0; (2.41) DEP k h c X i=0 hf i ( k ); k if i ( k )hf ext (g k ) = 0;; k = 0;:::;N (2.42) c X i=0 hf i ( k ); k iDf i ( k ) T k = 0; k = 0;:::;N (2.43) where k 2g is dened by h k ;i = I(d 1 h k ( k ))h(d h k ) ad (Ad (h k ) k+1) (d 1 h k ) I( k ); + D I( k );h D d 1 h k ( k ) E + * h (d h k ) N X i=k+1 Ad (h k )(h i1 ) (D g f ext (g i )g) T i ; + ; (2.44) k = Ad (h k ) k+1 k : (2.45) Note: The proposition denes (Nn + (N + 1)n + (N + 1)m) equations (2.40)-(2.43) in the (Nn + (N + 1)n + (N + 1)m) unknowns ( 0:N ; 0:N ; 0:N ). A solution can be found using standard nonlinear root nding. 2.8 The sub-Riemannian case Next we consider the case in which the body-xed velocity is restricted to lie in a m-dimensional distribution h g, where m<n. This problem is known as the the sub- Riemannian optimal control problem. The continuous version has been studied by many authors with the general case on Riemannian manifolds and compact Lie groups rst developed in [48], and extended in [23],[37] to the more general elastic sub-Riemannian 42 interpolation problem and more recently used in an interesting imaging application [25]. In this section we derive necessary conditions for optimality for discrete solution trajectories of a discrete dynamic nonholonomic principle. Assume that a basis for g can be constructed using the constant elements X i 2 g, i = 1;:::;n in such a way that the rst m elements span h, i.e. h = spanfX 1 ;:::;X m g; Assume that h is not a proper subalgebra of g but the iterated Lie brackets ofX 1 ;:::;X m span g so that the system is controllable. In order to make the derivations more clear, let X 1 ;:::;X n be orthonormal. The more general case can be treated with slight modication by the addition of normalizing terms and would not greatly aect the nal results. Let the orthogonal complement to h be dened by h ? , i.e. g = hh ? . The nonholonomic constraint 2h can be written as hh;X c ii = 0; c =m + 1;:::;n; (2.46) since h ? = spanfX m+1 ;:::;X n g. If we denote the velocity coordinates with respect to the basisfX i g by i , i.e. such that = i X i , then the constraint is simply c = 0: 43 2.8.1 Discrete Equations of Motion The discrete LDAP principle (2.4) can be easily extended to the sub-Riemannian case. The discrete path is formally denoted (g;;;) 0:N :ft k g N k=0 ! Ghg h ? . The only dierence with the holonomic case is the addition of the constraint (2.46) as well as restriction the allowable variationsg 1 k g k . The dynamic sub-Riemannian LDAP principle is formulated as: N1 X k=0 h 1 2 hI k ; k i + k ; 1 (g 1 k g k+1 )=h k + N X k=0 hTL g 1 k e f k ;g k i = 0; where g 1 k g k 2h; k 2h (2.47) where the discrete forces e f k 2 h are also aligned with the constraints. After taking variations in (2.47) we obtain the following discrete equations of motion. h(d 1 h k ) k (d 1 h k1 ) k1 e f k ;X i i;i = 1;:::;m; k = 1;:::;N 1; (2.48) hh k ;X i ii = 0; i =m + 1;:::;n; k = 0;:::;N 1; (2.49) h k ;X i i = 8 > > < > > : hI k ;X i i; i = 1;:::;m 0; i =m + 1;:::;n k = 0;:::;N 1; (2.50) g 1 k g k+1 =(h k ); k = 0;:::;N 1; (2.51) 44 Following eq. (2.50)-(2.49) the momentum and velocity can be written written in coordinates according to k := I 1 1 k ;:::;I m m k ; 0;:::; 0 ; k := 1 k ;:::; m k ; 0;:::; 0 : (2.52) 2.8.2 Necessary Conditions for Optimality Our goal is again to compute a discrete trajectory (g 0:N ; 0:N1 ) the minimizes the total control eort. The indirect method can be extended to the sub-Riemannian case by introducing additional multipliers k 2 h ? that enforce the constraint k 2 h without restricting variations on k . Dene the Lagrangian multipliers 2 g , k 2 h and the Hamiltonian function H( 0:N1 ;f 0:N ; 0:N ; 0:N1 ;) = N X k=0 1 2 k e f k k 2 +hDEP k e f k ; k i + N1 X k=0 h k ; k i +h; REC( 0:N1 )i; where the quantities DEP and REC were dened in Sec. 2.5. The necessary conditions for an optimal trajectory are dened in the following proposition (which extends Prop. 2). Proposition 4. The trajectory of a discrete mechanical system on a Lie group G with algebra g and Lagrangian`( k ) = 1 2 hI k ; k i, with xed initial and nal congurations and velocities g(0)2G, (0)2 h and g(T )2G, (T )2 h, minimizes the total control eort P N k=0 1 2 k e f k k 2 only if the discrete body-xed velocity curve 0:N1 satises the following 45 Necessary Conditions for Optimality (d 1 h k ) k (d 1 h k1 ) k1 = 0; k = 1;:::;N 1 (2.53) 1 ((h 0 )(h N1 ) (g(0) 1 g(T )) 1 ) = 0; (2.54) where k 2g is dened by h k ;i = I(d 1 h k ( k ))h(d h k ) ad Ad (h k ) e f ] k+1 (d 1 h k ) I( k ) + k ; + D I( k );h D d 1 h k ( k ) E ; (2.55) k = Ad (h k ) e f ] k+1 e f ] k ; (2.56) e f 0 = (d 1 h 0 ) I( 0 )I((0)); (2.57) e f k = (d 1 h k ) I( k ) (d 1 h k1 ) I( k1 ); k = 1;:::;N 1 (2.58) e f N =I((T )) (d 1 h N1 ) I( N1 ); (2.59) Note: The proposition denesNn equations (2.53)-(2.54) in theNn unknown coordinates [( 1 0 ;:::; m 0 ; ( 0 ) 1 ;:::; ( 0 ) c );:::; ( 1 N1 ;:::; m N1 ; ( N1 ) 1 ;:::; ( N1 ) c )), c =nm. A solu- tion can be found using standard nonlinear root nding. 2.9 Eciency Techniques There are several issues that are critical to the success of the optimization. One is the question of trajectory initialization and region of convergence. Another is the issue of the trajectory resolution and the balance between accuracy and eciency for optimization problems. 46 2.9.1 Trajectory Initialization We propose two ways to initialize a trajectory. None of these methods start with a near- optimal trajectory but the initial guesses satisfy the system dynamics and the imposed boundary conditions. The rst method is based on turning the controls on only in the beginning and end of the trajectory, and using constant velocity (geodesic) motion ev- erywhere else. The second is based on a quadratic interpolation in the Lie algebra to produce trajectories whose reconstruction to the group satises the boundary conditions exactly. 2.9.1.1 Geodesic Interpolation In the geodesic interpolation we simply nd a velocity that is constant everywhere but the rst and last segment and such that the boundary conditions are satised. k = 1 (h i )(g 1 i g f )(h f ) h(N 2) ; k = 1;:::;N 2 After computing the velocities i the dynamics equations can be satised by adjusting the forces which can be done by straightforward computation. Ideally, the map should be the exponential map since it \reconstructs" the trajectory exactly. In general, there is no closed form for the logarithm (since in this case 1 = log) of a matrix and approximations can be used (in the case of SO(3) and SE(3) there is a closed form and this is not an 47 issue). Alternatively, one can use the Cayley map but the initial trajectory would satisfy the boundary conditions only approximately. The map cay 1 :G!g is dened by cay 1 (g) =2(I +g) 1 (Ig) (2.60) The Cayley map is not a good choice if the initial and nal congurations g i and g f are far apart. Instead, one should use a higher order approximation to the exponential map and its inverse. 2.9.1.2 Lie Algebra Quadratic Interpolation Continuous Interpolation For the moment assume that we are working in a continuous setting and need to nd a trajectory that interpolates the boundary conditions at timest = 0 andt =T respectively. Dene the curve r : [0;T ]!g by r(t) = 0 t +c 1 t 2 =2 +c 2 t 3 =3 for some constant c 1 ;c 2 2g. Let the continuous trajectory have the form g(t) =g 0 (r(t)); Then, since (t) = g(t) 1 _ g(t)2 g and using the right tangent trivialization the velocity is (t) = d r(t) _ r(t) 48 Therefore, the constants c 1 and c 2 determine a family of curves. Among these curves it now becomes interpolating curves. Proposition 5. A curve (g;) : [0;T ]!Gg of the form (g(t);(t)) = (g 0 (r(t));d r(t) _ r(t)), where r(t) = 0 t +c 1 t 2 =2 +c 2 t 3 =3, t2 [0;T ], with c 1 ;c 2 2g dened by c 1 = 6 T 2 1 (g 1 0 g T ) d 1 r(T ) T 4 T 0 c 2 = 3 T 3 1 (g 1 0 g T ) 0 T 3 2T c 1 satises the given boundary conditions (g(0);(0)) = (g 0 ; 0 ) and (g(T );(T )) = (g T ; T ). Proof. The boundary conguration condition can be expressed asg 1 0 g T =(r(T )) which is equivalent to 1 (g 1 0 g T ) = 0 T +c 1 T 2 =2 +c 2 T 3 =3: The nal velocity boundary condition is equivalent to 0 +c 1 T +c 2 T 2 = d 1 r(T ) T The above two conditions are combined to give the closed form solution forc 1 andc 2 . Corollary 1. If 0 = T = 0 then c 1 = 6 T 2 1 (g 1 0 g T ); c 2 = 1 T c 1 49 and g(t) =g(0)(c 1 t 2 =2 +c 2 t 3 =3) =g 0 3t 2 T 2 2t 3 T 3 1 (g 1 0 g T ) Discrete Interpolation The discrete interpolation follows the continuous one very closely with the exception that the initial and nal segments with xed velocities ( i and f ) have to be treated specially. The trajectory is then constructed using k = 1 g(kh) 1 g((k + 1)h) ; k = 1;:::;N 2; and the dynamics equations satised by adjusting the forces correspondingly. 2.9.2 Trajectory Renement The eciency of the optimization depends on choosing an initial trajectory guess that is close to the optimal one. An obvious way to choose such an initial guess is by computing a lower resolution trajectory that approximates the desired solution. Such trajectory is easier to compute and at the same time can be used to construct a good initial guess for successively rened trajectories. This process is repeated until the desired resolution is reached. For example, Fig. 2.1 shows the renement of an initially coarse trajectory on SO(3). Each point along the trajectory is represented by its rotation axis frame. While we do not have detailed results on convergence and runtime eciency comparisons, such type of incremental renement experimentally results into tremendous computational savings. 50 Figure 2.1: Successive renement of an optimal trajectory on SO(3) 2.10 Applications 2.10.1 Rigid Body Reorientation on SO(3) Figure 2.2: Objective function convergence ratio vs. trajectory resolution. The graph shows average results from 3000 Monte Carlo simulations. We have implemented both the direct optimization method (dened by (2.16)) and the indirect one (dened by Prop. (2)). The direct method is implemented using SQP while the indirect using Newton's root nding method. Here we present results only from the direct formulation applied to a simple problem of reorienting a fully-actuated asymmetric rigid body on SO(3) by applying torques around its principal axes of inertia. Each solution is obtained using the Lie algebra quadratic 51 Figure 2.3: Runtime vs. trajectory resolution (normal scale on left and log scale on the right). The graph shows average results from 3000 Monte Carlo simulations. interpolation initialization and successive renement techniques described in Sec.2.9.1.2 and 2.9.2. Fig. 2.2 shows how well a trajectory approximates (converges to) an optimal one as the time step (or resolution) is increased. The results are averaged from 3000 Monte Carlo runs with random boundary conditions. Overall, 3% of the trials fail to converge. Fig. 2.3 shows the average runtime in milliseconds as a function of the resolution (the tests were performed on an average desktop PC with 1.8 Pentium IV CPU.) From both graphs one can conclude that a nearly optimal trajectory can be achieved on average with at least 100 time steps. It takes on average 1 second of computation time for a trajectory with such resolution. 2.10.2 Simple Helicopter in a Digital Terrain Consider the following model of a helicopter depicted in Fig. 2.4. The vehicle is modeled as a single underactuated rigid body on SE(3) with mass m and principal moments of rotational inertia J 1 ;J 2 ;J 3 . The inertia tensors are J = diag(J 1 ;J 2 ;J 3 ) and M = mI 3 . 52 collective yaw Figure 2.4: Simplied helicopter model used our tests. The system conguration and velocity are described in standard notation dened in Sec. 2.6.3. The system is subject to external force due to gravity f ext ((R;x); (!;v)) = (0; 0; 0;R T (0; 0;9:81m)): The vehicle is controlled through a collective u c (lift produced by the main rotor) and a yaw u (force produced by the rear rotor), while the direction of the lift is controlled by tilting the main blades forward or backward through a pitch p and a sideways roll r . The shape variables are = ( p ; r ), the controls are u = (u c ;u ): The resulting control covectors are f 1 () = (d t sin r ;d t sin p cos r ; 0; sin p ; cos p cos r ); f 2 () = (0; 0;d r ; 0;1; 0): The discrete equations of motion are then obtained by substituting these specic expres- sions into the general integrator derived in Sec. 2.7 using the SE(3) Lie group operators dened in Sec. 2.6.3. An optimal trajectory is obtained by solving the general neces- sary conditions for optimality dened in Prop. (3) using nonlinear root nding. Fig. 2.5 53 shows a control-eort minimizing trajectory for the simulated helicopter between two zero-velocity states in an articial canyon. The resulting state and control curves are shown in Fig. 2.8. Controllability In order to establish the controllability of the system one can use the good symmetric products [6] of the two vectors I 1 f 1 and I 1 f 2 where I is the SE(3) inertial tensor [I] = 2 6 6 4 J 0 0 M 3 7 7 5 ; and show that the system is locally conguration controllable at zero velocity. Since the actuator inputs have bounds, one has to design an algorithm which allows time to vary in order to accommodate these limits. In our implementation, the time-step h (and hence the nal time T ) is part of the optimization state vector and is allowed to vary within some prescribed bounds. Figure 2.5: Example of an optimized trajectory in a complex environment: a helicopter path through an outdoor canyon. 54 Figure 2.6: Top and side views of the helicopter trajectory shown in Fig. 2.5 -0.2 0 0.2 0.4 0.6 0.8 1 0 1 2 3 4 5 6 7 8 q time (s) Orientation qw qx qy qz 0 5 10 15 20 25 0 1 2 3 4 5 6 7 8 m. time (s) Position x y z Figure 2.7: The orientation and position as functions of time for the helicopter trajectory shown in Fig. 2.5. Obstacles The vehicle is required to stay away from obstacles by enforcing inequality constraints H i (R;p) = dist(A(R;p);O i )d s > 0, whereAR 3 is the region occupied by the robot,O i R 3 represent the static obstacles, and d s is some safety distance. The function dist computes the minimum distance between two rigid objects. In our implementation both the canyon and the helicopter are triangulated surfaces and we use the Proximity Query Package (PQP) to compute dist. 55 -2 0 2 4 6 8 10 12 14 0 1 2 3 4 5 6 7 8 force (N) time (s) Forces lift yaw -0.2 -0.15 -0.1 -0.05 0 0.05 0.1 0.15 0.2 0 1 2 3 4 5 6 7 8 rad time (s) Blade Angles pitch roll Figure 2.8: The control forces and blade angles as functions of time for the helicopter trajectory shown in Fig. 2.5. The optimization runs eciently in the absence of obstacles or with simple (smooth and convex) obstacles (taking in the order of a few seconds for most tests). On the other hand, complex rough terrains can slow down the system signicantly. Note also that our simple implementation faces the same robustness issues as many optimization procedures: a bad choice for the initial trajectory may lead to a local minima. One way to speedup convergence is to start with a good initial obstacle-free path computed, for example, using a local obstacle avoidance method (e.g. [17]). Nevertheless, as already pointed out by several similar works cited in our references, this approach should be used to produce small-scale local solutions that are combined by more global methods in a hierarchical fashion (e.g. see [35] regarding incremental and roadmap planners, as well as [22, 16] for planning using primitives). We explore such ideas in Ch. 4. An obvious application is also the renement of trajectories produced by discrete or sampling-based planners. Finally, in order to assess the exact numerical benets of this method, a detailed performance analysis and comparison to related methods is needed and is currently under way. 56 2.10.3 Boat subject to External Disturbances Consider a planar boat model shown in Fig. 2.9. The conguration space of the system is SE(2) with coordinates q = (;x;y) denoting orientation and position with respect to a xed global frame. Dene the body xed velocity 2se(2) by = [!;v;v ? ] T ; where! is the angular velocity (yaw),v is the forward velocity (surge),v ? is the sideways velocity (sway). damping damping thrusters wind Figure 2.9: Planar boat controlled with two thrusters, and subject to hydrodynamic damping and wind forces. The Lagrangian isl() =hM;i, whereM is the combined mass matrix of the boat's inertia and added mas of the surrounding uid. The system is controlled with two xed thrusters placed at the rear of the boat at distancec from the long axis of the boat 57 Figure 2.10: The USC RoboDuck2 boat used for experiments. producing forces u r and u l , respectively. These two forces result in the propulsion force f prop 2se(2) with respect to the boat-xed frame through the transformation f prop =B 2 6 6 4 u l u r 3 7 7 5 ; where B has the nominal form B = 2 6 6 6 6 6 6 4 c c 1 1 0 0 3 7 7 7 7 7 7 5 : For a more accurate model,B can be determined experimentally as a function of velocity to account for unmodeled center of mass shift. The boat is subject to hydrodynamic damping forces f diss : SE(2)se(2)! se(2) in the form f diss (g;) =R(g;); 58 where R : SE(2)se(2)! L(se(2);se(2) ) is a positive denite symmetric linear map. Other external forces such as wind and currents are denoted by f ext : SE(2)se(2)! se(2) . For example, the USC boat has an onboard wind sensor providing wind direction and eective force. The combined force acting on the boat is f =f prop +f diss +f ext : The continuous equations of motion are the Euler-Poincar e equations (2.3) on SE(2) in the form M 2 6 6 6 6 6 6 4 _ ! _ v _ v ? 3 7 7 7 7 7 7 5 = 2 6 6 6 6 6 6 4 0 0 0 v ? 0 ! v ! 0 3 7 7 7 7 7 7 5 T M 2 6 6 6 6 6 6 4 ! v v ? 3 7 7 7 7 7 7 5 +B 2 6 6 4 u l u r 3 7 7 5 R 2 6 6 6 6 6 6 4 ! v v ? 3 7 7 7 7 7 7 5 +f ext : The discrete equations are (2.7)-(2.9) with the corresponding operations on SE(2) dened in Sec. 2.6.2. We illustrate the optimal control of the boat in two scenarios with dierent wind forces and nal desired states. The boat parameters were chosen as M = diag(3; 1; 5); R = diag(1;:5; 10) with R 33 begin relatively high corresponding to the high lateral damping. 59 0 5 10 15 20 25 30 35 −2 −1 0 1 2 3 velocities m/s | rad/s sec ω v v − 0 5 10 15 20 25 30 35 −3 −2 −1 0 1 2 thruster forces sec N u left u right −1 0 1 2 3 4 5 6 0 1 2 3 4 5 0 5 10 15 20 25 30 35 −3 −2 −1 0 1 2 velocities m/s | rad/s sec ω v v − 0 5 10 15 20 25 30 35 −10 −5 0 5 10 thruster forces sec N u left u right Figure 2.11: Two optimal control scenarios with dierent boundary conditions and wind forces (shown as arrows along the path). The boat always starts at the origin (;x;y) = (0; 0; 0) with zero velocity and must arrive at the designated positions with zero velocity. Fig. 2.11 shows the path taken by the boat and the resulting velocity and control curves. The paths were discretized using N = 32 segments which results in real-time control performance. 60 Chapter 3 Nonholonomic Integrators Summary The chapter develops structure-preserving integrators for nonholonomic mechanical sys- tems through a discrete geometric approach. At the core of the formulation lies a dis- crete Lagrange-d'Alembert-Pontryagin variational principle. The focus of this work is on systems with symmetries, controllable shape (internal variables), and nonholonomic con- straints. Our discrete approach is especially well suited for such systems since it respects the structure of the state space and provides a framework for constructing more accurate and numerically stable integrators. The dynamics of the systems we study is derived by vertical and horizontal splitting of the variational principle with respect to a nonholo- nomic connection encoding the kinematic constraints and allowed symmetry directions. We formulate a discrete analog of this principle and derive discrete Euler-Poincar e and Euler-Lagrange equations resulting from the splitting of the principle with respect to a discrete approximation of the connection. A family of variational Euler-type integrators are then derived and applied to two examples{the steered robotic car and the snakeboard. 61 3.1 Introduction The goal of this chapter is to develop integrators for mechanical systems subject to non- integrable constraints on the velocities, i.e., nonholonomic constraints. We study systems that evolve on a conguration manifold Q = MG constructed from a Lie group G whose action leaves the kinetic energy invariant (and so G is a group of symmetries) and a vector space M that describes the system internal shape. This general cong- uration space applies to systems from several domains, e.g., locomotion systems found in nature [31, 39, 18], vehicles used in robotics and aerospace [46, 47, 3, 6], systems in molecular dynamics [26, 52]. Their dynamics is derived by explicitly factoring out the group invariance through reduction by symmetry, and consequently splitting the equations of motion into vertical{ corresponding to symmetries aligned with the constraints and dening the evolution of a momentum, and horizontal{dening the dynamics of the shape space. This has proven not only computationally benecial, from reducing the dimension and avoiding numerical ill-conditioning, but also crucial in studying the stability, controllability, and motion generation of such systems. In this thesis we focus on their proper discretization and propose geometric integrators that respect the state-space structure of the symmetries and constraints, preserve any invariants exhibited by the continuous system, and result in stable and accurate numerical schemes. We follow the approach of discrete mechanics [41] and derive discrete equations of motion of the system through the discretization of the underlying variational principles governing the dynamics. In particular, we employ a Lagrange-d'Alembert-Pontryagin 62 (LDAP) variational principle [53] that diers from a standard variational principle, such as Lagrange-d'Alembert's, by the presence of a new additional velocity variable v2T q Q at each point q2Q which by denition does not correspond to the rate of change of the conguration but this dependence is indirectly enforced using a kinematic constraint of the form _ qv = 0 and a multiplier p2T q Q corresponding to the momentum. We for- mulate a discrete version of this principle which, in addition to retaining all the desirable preservation properties inherent to the discrete variational approach, also simplies the construction of integrators. First, the Pontryagin viewpoint obviates the need to use a discrete LagrangianL d :QQ!R (which is the standard way to approximate the action integral in discrete mechanics, e.g. as formulated by Marsden and West[41]) as well as a discrete nonholonomic distributionD d QQ (introduced by Cortes [13]) since now a state along the discrete trajectory is not represented by a pair of points (q k ;q k+1 )2QQ but by (q k ;v k )2 TQ and both the original continuous Lagrangian and the constraints can be evaluated directly at such points to achieve the desired approximation. Second, the additional variables v and p give freedom in designing higher-order integrators by combining standard quadrature rules with higher order approximation of the kinematic constraint (as shown by Bou-Rabee and Marsden [5]). Further details about the principle are given in Sec. 2.3, and Sec. 3.2.3. The results presented here build upon previous work on the variational discretization of systems with symmetries, as well as systems with nonholonomic constraints. Bobenko and Suris [4] and Marsden et al. [42] rst studied the discrete Euler-Poincar e equations 63 for systems on Lie groups; Bou-Rabee and Marsden [5] extended those ideas in the frame- work of the Hamilton-Pontryagin principle in order to design more general and higher- order integrators; Jalnapurkar et al. [27] considered the discretization of the more general principle bundle case with abelian symmetry and its reduction using Routh's method. Nonholonomic constraints from a discrete variational viewpoint were rst studied by Cortes [13] who also considered the invariance of such systems with respect to Lie group actions and derived a momentum equation with properties consistent with the continuous case. M. de Leon et al. [14, 38] considered an alternative discretization of nonholonomic systems in terms of generating functions. Fedorov and Zenkov [19] extended the reduced discrete approach to systems on a Lie group to include nonholonomic constraints on the group and derived the so called Euler-Poincar e-Suslov equations. McLachlan and Perl- mutter [44] studied the general case of systems on vector spaces as well as on a group with nonholonomic constraints focusing on the time-reversibility and the importance of the preservation of invariants. Contributions. Our work extends the recently-introduced geometric Lie group inte- grators [5] to provide a systematic approach to the design of structure-respecting integra- tors for nonholonomic mechanical systems. While related to the work of several authors noted above, our discrete approach to nonholonomic systems with symmetries captures the geometry of general systems (dened in terms of principle bundle and nonholonomic connections) with arbitrary group structure, constraints, and shape dynamics, and is not restricted to a conguration space that is either solely a group or has a Chaplygin-type symmetry. As a result our formulation contains a discrete momentum equation and a set 64 of discrete reduced Euler-Lagrange equations analogous to the continuous case (e.g. as de- scribed in [2]) that explicitly account for and respect the interaction between symmetries and constraints in the vehicle dynamics. Organization. We start by recalling the geometry of nonholonomic systems with sym- metries. Then we introduce the nonholonomic LDAP principle for systems with symme- tries and derive the standard nonholonomic equations [2] that result from the splitting of the variational principle into vertical and horizontal parts with respect to the non- holonomic connection. The discrete analog of this principle is then formulated and the corresponding equations of motion derived. The vertical equation is equivalent to the evolution of a discrete momentum map whose properties are examined. Similar to a previous study by Cortes [13] we also consider the case of linear connection revealing an interesting link to the continuous dynamics. We apply the resulting algorithms to two examples: the steered car with simple dynamics and the snakeboard, and analyze the integrators accuracy and eciency. Finally, we use the discrete equations as constraints in an optimal control problem to generate optimal maneuvers useful for motion planning. 3.2 Nonholonomic Systems with Symmetry This section considers nonholonomic systems with symmetries in the continuous setting. We start by recalling standard concepts used to dene the state space geometry. Then we formulate the nonholonomic LDAP principle and obtain the continuous nonholonomic equations of motion. 65 Assume that : Q! Q=G is a principle bundle on the manifold Q with group G. The system has Lagrangian L : TQ! R and is subject to nonholonomic constraints dened by the regular distributionDTQ. A group orbit is a submanifold denoted by Orb(q) :=fgqj g2 Gg. If g is the Lie algebra of G, then T q Orb(q) =f Q (q)j 2 gg, where Q is the innitesimal generator corresponding to the Lie algebra element dened by Q (q) = d ds s=0 exp(s)q: Dene the subspacesV q ;S q ;H q according to V q =T q Orb(q); S q =D q \V q ; D q =S q H q : These denitions have the following physical meaning (see [3] for a more detailed descrip- tion): V q { space of tangent vectors parallel to symmetry directions, i.e. the vertical space; S q { space of symmetry directions that satisfy the constraints; H q { space of tangent vectors that satisfy the constraints but are not aligned with any directions of symmetry, i.e. the horizontal space. We make the following additional assumptions that are standard in the literature (see [3, 10]) Dimension Assumption: For each q2Q, we have T q Q =D q +V q . Invariance of L: The Lagrangian L is G-invariant. 66 Invariance ofD: The distributionD is G-invariant. Dene the vector space s q T q Q=G to be the set of Lie algebra element whose innitesimal generators lie inS q , i.e. the space of symmetry directions that satisfy the constraints, by s q =f(q)j Q (q)2S q g: The bundle with bers s q at all q2Q is denoted s. Since our main interest is in a conguration space that is by construction of the form Q = MG we will restrict any further derivations to the trivial bundle case. While the more general case (introduced so far) can be treated in an analogous manner with slight modication we stick to the trivial case for clarity without loosing the general applicability of our results. Using (global) trivial bundle coordinates (r;g)2 MG we have Q (r;g) = (0;(r;g)g)2S q . If we denote the basis for s q byfe b (r;g)g, for b = 1;:::; dim(S) then sinceD is G-invariant g can be factored out from this basis, i.e. e b (r;g) = Ad g e b (r), wherefe b (r)g is the body-xed basis. We denote s r the space spanned byfe b (r)g. Lastly, the system is subject to control forcef : [0;T ]!T M restricted to the shape space. 67 3.2.1 Nonholonomic Connection With these denitions we can dene a principle connectionA :TQ! g with horizontal distribution that coincides withH q at pointq. This connection is called the nonholonomic connection and satises A(r;g) ( _ r; _ g) = Ad g (g 1 _ g +A(r) _ r); (3.1) whereA(r) is its local form. The nonholonomic connection is constructed according toA =A kin +A sym , where A kin is the kinematic connection enforcing the nonholonomic constraints andA sym is the mechanical connection corresponding to symmetries in the constrained directions (i.e. the group orbit directions satisfying the constraints). These maps satisfy g 1 _ g +A kin (r) _ r = 0; A sym (r) _ r = ; g 1 _ g +A(r) _ r = : (3.2) where 2 s r is called the locked angular velocity, i.e. the velocity resulting from in- stantaneously locking the joints described by the variables r. Intuitively, when the joints stop moving the system continues its motion uniformly around a curve (with tangent vectors inS) with body-xed velocity and a corresponding spatial momentum that is conserved. 68 The vector ver r _ q = (0; )2 (TMs) r is the vertical component relative the combined connectionA and hor r _ q = ( _ r;A(r) _ r)2 (TMg) r is the horizontal component. Velocity vectors on TQ=GTMg are split according to ( _ r;g 1 _ g) r = ver r _ q + hor r _ q = (0; ) + ( _ r;A(r) _ r): 3.2.2 Vertical and Horizontal Variations We now consider variations of the conguration variables in the vertical and horizontal directions. Following [9, 3] dene the following Denition 1. Vertical variations Consider variations (r;g) such that r = 0 and gg 1 =A(r;g) (r;g)2s (r;g) . Then (0;gg 1 )2T r Ms (r;g) is clearly vertical and so is (0;g 1 g)2T r Ms r . This can be easily checked considering that for trivial bundles s can be constructed using a basisfe b :Q!sg such that e b (r;g) = Ad g e b (r), where e b (r) is a body-xed basis in a left-trivialization. Denition 2. Horizontal variations Variations satisfyingA(r;g) (r;g) = 0, or equiv- alentlyg 1 g +A(r)r = 0, result in variations (r;g 1 g) = (r;A(r)r)2 (TMg) r that are horizontal. Since vertical and horizontal variations can be taken independently we can consider variational principles based on vertical and horizontal variations separately. The next sec- tion presents these principles and the derivation of the resulting nonholonomic equations of motion. 69 3.2.3 Lagrange-D'Alembert-Pontryagin Nonholonomic Principle Dene the reduced Lagrangian ` :TMg!R according to `(r; _ r;) =L(r; _ r;e;g 1 _ g); and the constrained reduced Lagrangian l c :TMs!R such that l c (r; _ r; ) =`(r; _ r; A(r) _ r): While both reduced Lagrangians capture the group invariance of the system, using the constrained reduced Lagrangian l c has several advantages. One is that, unlike , the locked angular velocity diagonalizes the kinetic energy which has important implications in studying the stability of the system [40]. Another is that in the resulting equations of motion the rate of change of the generalized momentum decouples from that of the shape variables which is key in exploiting the holonomy of the system for locomotion and motion planning purposes. Next, we begin from the general formulation of the LDAP principle (ref) and extend it to the principle bundle setting introduced earlier. Then we formulate two equivalent reduced principles, rst in terms of the reduced Lagrangian ` and then in terms of the constrained reduced Lagrangian l c . Using bundle coordinates (r;g)2MG and denoting tangent vectors (u;w)2TM TG and corresponding momenta (p; ^ p)2T MT G we can rewrite the nonholonomic principle (2.4) as: 70 Denition 3. Nonholonomic LDAP Principle (unreduced) Z T 0 fL(r;g;u;w) +hp; _ rui +h^ p; _ gwig dt + Z T 0 hf;ridt = 0 subject to: nonholonomic constraint A(r;g) (u;w) = Ad g ; vertical variations s.t. (r;g 1 g) = (0;); where 2s r and, horizontal variations s.t. (r;g 1 g) = (r;A(r)r): (3.3) After substituting the reduced Lagrangian` and the constraint (eq. 3.1), and dening the local momentum =TL g ^ p2g the principle can be equivalently expressed as Denition 4. Reduced Nonholonomic LDAP Principle (reduced Lagrangian `). Using the notation := A(r)u, the principle requires that Z T 0 f`(r;u;) +hp; _ rui +h;g 1 _ gig dt + Z T 0 hf;ridt = 0 subject to: vertical variations s.t. (r;g 1 g) = (0;); where 2s r and, horizontal variations s.t. (r;g 1 g) = (r;A(r)r): (3.4) In the above formulation variations u, , p, are free. The version of the principle in terms of the constrained Lagrangian is then 71 Denition 5. Reduced Nonholonomic LDAP Principle (constrained reduced Lagrangian l c ) Z T 0 fl c (r;u; ) +hp; _ rui +h;g 1 _ g +A(r)u ig dt + Z T 0 hf;ridt = 0 subject to: vertical variations s.t. (r;g 1 g) = (0;); where 2s r and, horizontal variations s.t. (r;g 1 g) = (r;A(r)r): (3.5) The principle (3.5) now contains all information necessary to derive the equations of motion that explicitly account for the symmetries, nonholonomic constraints, and their interaction. Lie algebra basis In practice, the constrained symmetry spaceS (r;g) often must be generated from Lie algebra basis that depends on the shape r 2 M. Therefore, we assume that the basisfe a (r)ja = 1;:::; dim(G)g spans g r , in such a way thatfe b (r)jb = 1;:::; dim(s)g is an orthogonal basis for s r at each r. Then 2 s r in this basis is = b e b (r). Derivation Taking arbitrary variations r, g, u, , p, in (3.5) and noting that (g 1 _ g) = _ + g 1 _ g; ; where =g 1 g; we obtain respectively 72 r) @l c @r ;r + D p; _ r E +h;DA(r)(r;u)i +hf;ri (3.6) g) + ; _ + g 1 _ g; (3.7) u) +h @l c @u ;ui +hp;ui +h;A(r)ui (3.8) ) + @l c @ ; h; i (3.9) p) +hp; _ rui (3.10) ) + ;g 1 _ g +A(r)u = 0 (3.11) Since u, , p, and are free we immediately obtain from (3.8)-(3.11) @l c @u p;r +h;A(r)ri = 0 (3.12) @l c @ = 0 (3.13) _ ru = 0 (3.14) g 1 _ g +A(r)u = 0 (3.15) Next we consider vertical and horizontal variations of (r;g) separately. 73 3.2.3.1 Vertical Equations Vertical variations have the form r = 0, g 1 g = ; where 2 s r , or in the previously dened basis = b e b (r). Therefore, after substituting the constraint (3.15), (3.7) gives ; d dt ( b e b (r)) + h A(r)u; b e b (r) i = 0: (3.16) Next, we dene the momentum components b =h;e b (r)i and, after integrating by parts, and using the boundary conditions g(0) = g(T ) = 0 we obtain their time-derivative according to _ b = ; @e b (r) @r _ r + ad A(r)u e b (r) ; since b are arbitrary. 3.2.3.2 Horizontal Equations Horizontal variations are constrained according to g 1 g = , where =A(r)r for variations r in the base. Dierentiating (3.12) with respect to time we get h _ p;ri = d dt @l c @u ;r +h _ ;A(r)ri +h;DA(r)( _ r;r)i 74 Therefore, after integrating by parts (3.6) and (3.7), using the boundary conditions r(0) =r(T ) = 0, and substituting =A(r)r we get @l c @r ;r d dt @l c @u ;r h _ ;A(r)rih;DA(r)( _ r;r)i +h;DA(r)(r;u)i +hf;ri +h _ ;A(r)rih[ A(r)u;A(r)r]i = 0 Simplifying, the horizontal (reduced Euler-Lagrange) equations of motion become @l c @r d dt @l c @u +f;r =h;B(r)(u;r) + ad A(r)ri; (3.17) where B(r)(u;r) =DA(r)(u;r)DA(r)(r;u) [A(r)u;A(r)r] is the curvature of the nonholonomic connectionA. Summary The equations of motion due to the splitting of the variational principle are g 1 _ g = A(r)u; (3.18) _ r =u; (3.19) = @l c @ ; (3.20) _ b = ; @e b (r) @r u + ad A(r)u e b (r) ; (3.21) @l c @r d dt @l c @u +f;r =h;B(r)(u;r) + ad A(r)ri (3.22) 75 for b = 1;:::; dim(s). These equations are standard in the literature on nonholonomic systems with sym- metries (e.g. [2, 34, 10, 3]) and we have managed to obtain them here directly from a reduced variational principle by restricting the variations on the conguration variables only. This is in contrast to the approach of separately studying the evolution of a mo- mentum map (e.g. as taken in [2]) or by additionally restricting the allowable variations on the velocity variables or (explored in [10, 8]). Our main motivation for this al- ternative intrinsic formulation is that such a self-contained principle can be easily cast in a discrete framework and we expect that the resulting discrete equations of motion would most closely preserve the variational, geometric structure of the original system. We develop the discrete framework in the next section. 76 M r k−1 r k r k+1 u k−1 u k r k−1+α r k+α r k+1 −r k =hu k G g k−1 g k g k+1 g k−1+α ξ k−1 g k+α ξ k g k−1+α g k+α τ −1 (g −1 k g k+1 ) =hξ k Figure 3.1: Discrete approximation (dashed) of continuous trajectories (solid) in the shape space (left) using linear interpolation, and in the group (right) using local geodesics dened by the ow of the map . The discrete velocity vectors shown approximate the average velocity along the segment and satisfy the constraint as dened underneath the gures. These velocity vectors are attached at quadrature points determined by the choice of 2 [0; 1]. 3.3 Geometric Discretization of Nonholonomic Systems In this section we formulate a discrete variational principle and derive a family of simple nonholonomic integrators that account for the group structure and constraint distri- bution, respect the work-energy balance, and have discrete momentum equation and a corresponding momentum map with properties analogous to the continuous case. 3.3.1 Discrete Approximation As we noted earlier the discrete mechanics approach is based on varying discrete trajecto- ries in order to nd critical values of an action integral approximated through quadrature. The approximation scheme can be simple, i.e. by joining the discrete points along the path with simple local interpolation and few quadrature evaluations along the segments; or they can be higher-order by further discretizing each segment and performing multiple quadrature computations. Here, for clarity we focus on one simple type of discretiza- tion termed variational Euler [5] and provide the complete higher-order formulation in Appendix (ref). 77 Discrete Trajectory The discrete LDAP framework is dened in terms of a discrete trajectory whose states are elements of the tangent spaces in the reduced bundle of velocities and momenta. The trajectory is formally dened as follows (see also Fig. 3.1) Denition 6. The discrete reduced path is denoted (r;u;p;g; ;) d :ft k g N k=0 ! (TMT M)Gsg and is subject to the constraints r k+1 r k =hu k ; 1 (g 1 k g k+1 ) =h k ; where k = k A(r k+ )u k , with r k+ := (1)r k +r k+1 for a chosen 2 [0; 1] and the map : g! G represents the dierence between two congurations in the group by an element in its algebra (see Sec. 2.3) The discrete control force is f 0:N :ft k g N k=0 !T M approximating a force controlling the shape. Based on this simple approximation, the continuous and discrete state variables are related through (Fig. 3.1): (r(t k+ ); _ r(t k+ )) (r k+ ; (r k+1 r k )=h) (g(t k+ ); _ g(t k+ )) g k+ ;g k+ 1 (g 1 k g k+1 )=h ; (3.23) where t k+ := t 0 +h(k +), g k+ = g k ( 1 (g 1 k g k+1 )) for each k = 0;:::;N 1 and 2 [0; 1]. 78 Constraint Consistency It is important to note that the discretization constraints between congurations and velocities from Dfn. 6 are invariant to left translations of the discrete trajectory. Left-translating a pair of congurations (g k ;g k+1 ) used to dene velocity k is equivalent to applying the lifted left action to k itself, i.e. (g k ;g k+1 ) =)g k+ 1 (g 1 k g k1 ) =g k+ k ; (g 0 g k ;g 0 g k+1 ) =)g 0 g k+ 1 ((g 0 g k ) 1 (g 0 g k1 )) =g 0 g k+ k : Therefore, the approximation (3.23) remains valid in a left trivialization e;g 1 (t k+ ) _ g(t k+ ) e; 1 (g 1 k g k+1 )=h ; and the left-invariant discrete body-xed velocity k can be used for discrete reduction analogous to the continuous case. Connection Equivariance Similarly to (3.23) the nonholonomic connection is approx- imated as A(r(t k+ );g(t k+ )) ( _ r(t k+ ); _ g(t k+ ))A(r k+ ;g k+ ) (u k ;g k+ k ) = Ad g k+ ( k +A(r k+ )u k ) = Ad g k+ k ; 79 for all k = 0;:::;N 1 and 2 [0; 1]. Note that the discretization constraint is also consistent with the required equivariance of the connection: Ad g 0A(r k+ ;g k+ ) (u k ;g k+ k ) = Ad g 0 Ad g k+ ( 1 (g 1 k g 0 1 g 0 g k+1 ) +A(r k+ )u k ) = Ad g 0 g k+ ( 1 ((g 0 g k ) 1 g 0 g k+1 ) +A(r k+ )u k ) =A(r k+ ;g 0 g k+ ) (u k ;g 0 g k+ k ): 3.3.2 Discrete Reduced LDAP Nonholonomic Principle We formulate the discrete version of the LDAP principle (3.5) by approximating the action integral along each discrete segment using a single evaluation determined by the choice of 2 [0; 1]. Denition 7. Discrete Reduced LDAP Principle N1 X k=0 h [fl c (r k+ ;u k ; k ) +hp k ; (r k+1 r k )=hu k i +h k ; 1 (g 1 k g k+1 )=h +A(r k+ )u k k ig + N1 X k=0 [hhf k+ ;r k+ i] = 0 subject to: vertical variations s.t. (r k ;g 1 k g k ) = (0; k ); where k 2s r k and, horizontal variations s.t. (r k ;g 1 k g k ) = (r k ;A(r k )r k ); (3.24) In the above formulation variationsu k , k ,p k , k are free. Allowing the Lagrangian and the connection to be evaluated atr k+ gives design freedom. Standard values of are 0, 0:5, 1. Setting = 0:5 provides more accurate approximation of the base dynamics 1 1 Note that setting = 0:5 is not equivalent to unreduced midpoint rule since in left trivialization there is no notion of midpoint on the group 80 while = 0; 1 results in less accurate, but simpler integrators. As we mentioned, there are more general ways to dene the discrete principle that allows arbitrary high approx- imation order but here we limit the exposition to lower order integrators for clarity. The discrete force f k+ = (1)f k +f k+1 is used to approximate the work done by f in a manner consistent with the rest of the discretization, i.e. through Z (k+1)h kh hf;ridthhf k+ ;r k+ i: Taking variations r k , g k , u k , k , p k , k in (3.24) and noting that ( 1 (g 1 k g k+1 )) = d 1 h k ( k + Ad g 1 k g k+1 k+1 ); where k =g 1 k g k , k = 1 (g 1 k g k+1 )=h, and d :g!g is the right-trivialized tangent of () dened by D() = TR () (d ) and d 1 : g! g is its inverse, we obtain respectively 81 r k ) h @l c k1+ @r + (1) @l c k+ @r ;r k +hp k +p k1 ;r k i +hh k1 ;DA k1+ (r k ;u k1 )i +hh k ; (1)DA k+ (r k ;u k )i +hh(1)f k1+ +f k+ ;r k i (3.25) g k ) + D (d 1 h k ) k + (d 1 h k1 ) k1 ; k E (3.26) u k ) +h @l c k+ @u ;u k +hhp;u k i +hh k ;A(r k+ )u k i (3.27) k ) +h @l c k+ @ ; k hh k ; k i (3.28) p k ) +hhp k ; (r k+1 r k )=hu k i (3.29) k ) +h k ; 1 (g 1 k g k+1 )=h +A(r k+ )u k k = 0; (3.30) where l c k+ :=l c (r k+ ;u k ; k ). Since u k , k , p k , and k are free we immediately obtain from (3.27)-(3.30) @l c k+ @u p k +A(r k+ ) k = 0 (3.31) @l c k+ @ k = 0 (3.32) (r k+1 r k )=hu k = 0 (3.33) 1 (g 1 k g k+1 )=h +A(r k+ )u k k = 0 (3.34) Next we consider vertical and horizontal variations of (r k ;g k ) separately. 82 3.3.2.1 Vertical Equations Vertical variations (Sec. 3.2.2) are of the form r k = 0, g 1 k g k = k , where k 2 s r k , or in the previously dened basis k = b k e b (r k ). Therefore, after substituting the con- straint (3.34), (3.26) gives D (d 1 h( k A(r k+ )u k ) ) k + (d 1 h( k1 A(r k1+ )u k1 ) ) k1 ; b k e b (r k ) E = 0 If we dene the discrete Euler-Poincar e operator DEP according to DEP (k) := (d 1 h( k A(r k+ )u k ) ) k (d 1 h( k1 A(r k1+ )u k1 ) ) k1 ; (3.35) then since b k are arbitrary, the vertical equations become hDEP (k);e b (r k )i = 0; (3.36) for b = 1;:::; dim(S), and k = 1;:::;N 1. 3.3.2.2 The Discrete Momentum Map Next, we dene a discrete momentum map, examine its properties and compare it to its continuous analog. It is well-known that for the nonholonomic systems that we consider the momentum, even in the direction of constrained symmetries, is not conserved in general. Instead, the momentum equation denes how the momentum components evolve in time. In the discrete setting the vertical equation (or the discrete momentum equation) is its analog. 83 Similar to the continuous setting the discrete vertical equation can be viewed as dening the evolution of a discrete momentum map that we dene next. Denition 8. Discrete Nonholonomic Momentum Map Dene the local discrete momentum map J loc :TMg!g by J loc (r k ;u k ; k ) = (d 1 h k ) k ; where k = @` @ (r k ;u k ; k ); and the spatial discrete momentum map J :TQ!g through J(r k ;u k ;g k ;v k ) := Ad g 1 k J loc (r k ;u k ;g 1 k v k ); where (r k ;u k )2TM and (g k ;v k )2TG. With these denitions we can compute the evolution of the discrete momentum map along symmetry directions that are allowed by the constraints, i.e. along the elements of the basisfe b (r;g)g at point (r;g)2 Q, for b = 1;:::; dim(S). Note that this basis is constructed from a body-xed basisfe b (r)g according to e b (r;g) = Ad g e b (r). For all such e b :Q! s we dene the momentum map components J nh b (r k ;u k ;g k ;v k ) at point k by J nh b (r k ;u k ;g k ;v k ) :=hJ(r k ;u k ;g k ;v k );e b (r k ;g k )i =hJ loc (r k ;u k ;g 1 k v k );e b (r k )ii: Proposition 6. Discrete Momentum Map Change. 84 G rk g rk e e b (r k ) J loc k s rk r k−1 r k M G rk−1 g rk−1 e e b (r k−1 ) J loc k−1 s rk−1 Ad ∗ g −1 k−1 gk J loc k−1 change of basis balance of momentum projected onto s rk Figure 3.2: Evolution of the discrete momentum map. At pointr k1 the map is computed by projecting the covector J loc k1 onto s r k1 dened by the basis e b (r k1 ); then in the Lie algebra basis attached at r k the covector J loc k1 transforms by Ad g 1 k g k+1 and the change in the momentum map is computed by subtracting it from the next momentum J loc k and projecting onto s r k (the notation J loc k := J loc (r k ;u k ; k ) was used with covectors drawn pointing towards the vectors that they act on). 85 The momentum components J nh b evolve along discrete LDAP solution trajectories ac- cording to J nh b (r k ;u k ;g k ;v k ) J nh b (r k1 ;u k1 ;g k1 ;v k1 ) =hJ(r k1 ;u k1 ;g k1 ;v k1 );e b (r k ;g k )e b (r k1 ;g k1 )i: Proof. Rewriting the momentum equation (3.36), derived form the discrete LDAP prin- ciple, in terms of the momentum map we obtain hJ loc (r k ;u k ; k ) Ad (h k1 ) J loc (r k1 ;u k1 ; k1 );e b (r k )i = 0; which is a momentum map balance equation depicted in Fig. 3.2. In spatial frame it reads hJ(r k ;u k ;g k ;g k k ) J(r k1 ;u k1 ;g k1 ;g k1 k1 );e b (r k ;g k )i = 0 which yields the component dierence. Corollary 2. Properties of the momentum map 1. The map components J nh b are not conserved in general. 2. Ife b (r) are independent ofr, then the discrete momentum equations are the discrete Euler-Poincar e equations projected onto the constraint symmetry space s. 3. If e b (r) are independent of r and if G is abelian then J nh b are constant along the discrete trajectory. This follows from the equality e(r k ;g k ) =e(r k1 ;g k1 ) since in this special case e b (r k ) =e b (r k+1 ) and Ad g = Id. 86 3.3.2.3 Horizontal Equations Horizontal variations (Sec. 3.2.2) are constrained according to g 1 k g k = k , where k = A(r k )r k for variationsr k in the base. With this denition of k and after substituting p k from (3.31) into (3.25) we get h @l c k1+ @r + (1) @l c k+ @r @l c k+ @u + @l c k1+ @u +h (f k1+ + (1)f k+ );r k =h k ;A(r k+ )r k ih k1 ;A(r k1+ )r k i hh k1 ;DA k1+ (r k ;u k1 )ihh k ; (1)DA k+ (r k ;u k )i hDEP (k);A(r k )r k i (3.37) for k = 1;:::;N 1. 3.3.2.4 The case of linear connection Next we study the special case when the connectionA(r) is linear in the base point r. This case is useful in comparing the resulting integrator to the continuous case in order to gain insight into the eect of discretization. Assume thatA(r) is linear. The following expressions then trivially hold A(r k+ )r k =A(r k )r k +h(1)DA k+ (u k ;r k ); A(r k1+ )r k =A(r k )r k hDA k1+ (u k1 ;r k ); 87 and after substituting them into (3.37) and using = exp the horizontal equations become @l c k1+ @r + (1) @l c k+ @r 1 h @l c k+ @u @l c k1+ @u +f k1+ + (1)f k+ ;r k = * k1 ;dA k1+ (u k1 ;r k ) 1 X i=1 B i i! ad i k1 A(r k1+ )u k1 A(r k )r k + + (1) * k ;dA k+ (u k ;r k ) 1 X i=1 B i i! ad i k A(r k+ )u k A(r k )r k + (3.38) where the curvature covariant derivative dA is dened by dA(u;r) =DA(u;r)DA(r;u); and B i are the Bernoulli numbers with the rst few given by B 1 =1=2, B 2 = 1=6, B 3 = 0; ... There are several special cases that lead to further simplication of the horizontal equations. The Lie bracket in (3.38) vanishes, for instance, when G is abelian; when g is one-dimensional; or whenever andA(r)u lie in the same one-dimensional vector space for all , r, and u (as in the snakeboard example from Sec. 3.4.2). Proposition 7. If the connectionA(r) is linear and the ad operator in (3.38) maps to 0 along the path, then the non-conservative forces on the right-hand side of the reduced discrete Euler-Lagrange equations (3.37) match the continuous case exactly. 88 Proof. If the ad operator maps to 0 then the curvature equals the covariant derivative, i.e. B = dA. Then if we denote the continuous gyroscopic force by F A (r;u;) = h;dA(u;r )i, the discrete forces on the right-hand side of (3.38) become F A (r k1+ ;u k1 ; k1 ) + (1)F A (r k+ ;u k ; k ) exactly representing the continuous force acting on the left and the right (depending on the value of ) of the ber at r k . This claim is analogous to the result obtained by Cortes [13] for Chaplygin-type symmetries and, as noted by the same author, if the gyroscopic forces vanish then the horizontal equations become a decoupled variational integrator on their own. Prop. 7 asserts that under similar conditions (linearity of the connection and vanishing of the bracket) this is also true for systems with nontrivial intersection between the constraints and symmetries. 89 3.3.2.5 Summary The discrete equations of motion are g 1 k g k+1 =(h( k A(r k+ )u k )); r k+1 r k =hu k ; k = @l c k+ @ ; hDEP (k);e b (r k )i = 0; h @l c k1+ @r + (1) @l c k+ @r @l c k+ @u + @l c k1+ @u +h (f k1+ + (1)f k+ );r k =h k ;A(r k+ )r k ih k1 ;A(r k1+ )r k i hh k1 ;DA k1+ (r k ;u k1 )ihh k ; (1)DA k+ (r k ;u k )i hDEP (k);A(r k )r k i (3.39) for b = 1;:::; dim(S) and k = 1;:::;N 1. Horizontal Equations (reduced Lagrangian `) It is also useful to express the base equations in terms of the reduced Lagrangian ` as well since our discrete formulation in general does not decouple the instantaneous change in the momentum components from that of the shape variables. While deriving the integrator in terms of l c is more 90 appropriate for analysis often working with the reduced Lagrangian ` results in simpler implementation. In that case the horizontal equations are @` k+ @u @` k1+ @u h @` k1+ @r + (1) @` k+ @r =A(r k ) DEP (k) +h (f k1+ + (1)f k+ ): (3.40) 3.4 Examples 3.4.1 Car with simple dynamics (x,y) θ φ ψ torques Figure 3.3: Car: pose & shape space variables. We study the kinematic car model dened in [31] with added simple dynamics (Fig. 3.3). The conguration space isQ =S 1 S 1 SE(2) with coordinatesq = ( ;;;x;y), where (;x;y) are the orientation and position of the car, is the rolling angle of the rear wheels, and is dened by = tan() where is the steering angle. The car has mass m, rear wheel inertiaI, rotational inertiaK, and we assume that the steering inertia is negligible. 91 The car is controlled by rear wheels torque f and steering velocity u . The Lagrangian is then expressed as: L(q; _ q) = 1 2 I _ 2 +K _ 2 +m( _ x 2 + _ y 2 ) ; and the constraints (see [31]) are cosdx + sindy =d ; sindx + cosdy = 0; d = l d ; where l is the distance between front and rear wheel axles, and is the radius of the wheels. These constraints simply encode the fact that the car must turn in the direction in which the front wheels are pointing, that the car cannot slide sideways, and that the change in orientation is proportional to the steering angle and turning rate of the wheels. Note now that for any element g = (;a;b) ofSE(2), the action g (q) = ( ; L ; + ;a + cos()x sin()y;b + sin()x + cos()y) leaves the Lagrangian and constraints invariant. As the shape coordinates are r = ( ;), the reduced Lagrangian thus becomes `(r;u;) = 1 2 0 B B B B B B @ u T 2 6 6 4 I 0 0 0 3 7 7 5 u + T 2 6 6 6 6 6 6 4 K 0 0 0 m 0 0 0 m 3 7 7 7 7 7 7 5 1 C C C C C C A : 92 The matrix representation of the connectionA dependent on r becomes: [A(r)] = 2 6 6 6 6 6 6 4 l 0 0 0 0 3 7 7 7 7 7 7 5 (3.41) This model is an example of the principle kinematic case in which the constraint distribution complements the space tangent to the group orbits. This is easily seen noting that D q = span @ @ ; @ @ ; V q = span @ @x ; @ @y ; @ @ Thus,S =; and there is no momentum equation. Continuous equations of motion The resulting continuous equations of motion are _ x = cos _ ; _ y = sin _ ; _ = l _ ; I +m 2 + K 2 2 l 2 _ u = K 2 _ _ l 2 +f ; _ =u ; Car Integrator The discrete equations of motion will be derived by substituting the Lagrangian and the connection of the steered car into (3.39). Dene u = (u ;u ) and =A(r)u and pick = exp. The expressionhDEP exp (k);A(r k )r k i involves the term 93 dexp 1 dened in (2.13). Observing that in the case of the carhad A(r)u ;A(r)i = 0 for any 2h and u;2TM and therefore h(dexp 1 ) ;A(r)i =h;A(r)i and since the operator DEP exp simplies to hDEP exp (k);A(r k )i :=h k k1 ;A(r k )i; the equations of motion simplify to g k+1 =g k exp(hA(r k+ )u k ); r k+1 =r k +hu k ; @` k+ @u @` k1+ @u =A(r k ) T ( k k1 ) +h (f k1+ + (1)f k+ ); for k = 1;:::;N 1. The exponential and Cayley maps for G = SE(2) are given in Sec. 2.6.2. The equations of motion can now be derived by substituting = @` @ = (K 1 ;m 2 ; 0); @` @r = (0; 0); @` @u = (Iu ; 0); 94 The exact equations are x k+1 x k = 8 > > < > > : v k ! k (sin( k +h! k ) sin k ) if !6= 0; cos k hv k if ! = 0: y k+1 y k = 8 > > < > > : v k ! k ( cos( k +h! k ) + cos k ) if !6= 0; sin k hv k if ! = 0: k+1 = k +h! k ; k+1 = k +hu k ; (I + 2 m)(u k u k1 ) + 2 K l 2 k ( k+ u k k1+ u k1 ) =h f k1+ + (1)f k+ ; where v k =u k , ! k = (=l) k+ u k . Thus, the integrator is easily computed as it is fully explicit for any choice of quadrature point . Figure 3.4: Stability and eciency of our nonholonomic integrator for car trajectories: averaged over 50 runs using a large range of initial conditions and steering commands, our nonholonomic integrator remains as accurate as RK2, at a fraction of the computational complexity. Numerical Comparisons Our numerical comparisons for nonholonomic motions are based on one-minute trajectories of the car with simple dynamics. The vehicle is con- trolled using sinusoidal inputs of frequency and amplitude designed to produce nontrivial paths such as parallel parking, sharp turns, and winding maneuvers. Since the trajectory 95 is relatively short allRK methods up to fourth order that we test (Fig. 3.4) are stable due in part to the simpler group structure of SE(2). Yet, our integrator performs almost as equally well asRK2, at half the computational time|due to its explicit update scheme. 3.4.2 The Snakeboard (x,y) θ ψ φ φ Figure 3.5: Snakeboard: pose & shape space variables. The snakeboard is a wheeled board closely resembling the popular skateboard with the main dierence that both the front and the rear wheels can be steered independently. This feature causes an interesting interplay between momentum conservation and the nonholonomic constraints: the rider is able build up velocity without pushing o the ground by transferring the momentum generated by a twist of the torso into motion of the board coupled with steering of the wheels through pivoting of the feet. When the steering wheels stop turning the systems moves along a circular arc and the momentum around the center of this rotation is conserved. A robotic version of the snakeboard also exists, equipped with a momentum-generating rotor and steering servos [45]. The shape space variables of the snakeboard are r = ( ;)2 SS denoting the rotor angle and the steering wheels angle, while its conguration is dened by (;x;y) denoting orientation and position of the board (see Figure 3.5). This corresponds to 96 a conguration space Q = SSSE(2) with shape space M = SS and group G = SE(2). Additional parameters are its mass m, distance l from its center to the wheels, and moments of inertia I and J of the board and the steering. The kinematic constraints of the snakeboard are: l cosd sin( +)dx + cos( +)dy = 0; l cosd sin()dx + cos()dy = 0; enforcing the fact that the system must move in the direction in which the wheels are pointing and spinning. The constraint distribution is spanned by three covectors: D q = span @ @ ; @ @ ;c @ @ +a @ @x +b @ @y ; where a =2l cos cos 2 ; b =2l sin cos 2 ; c = sin 2: The group directions dening the vertical space are: V q = span @ @ ; @ @x ; @ @y ; and therefore the constrained symmetry space becomes: S q =V q \D q = span c @ @ +a @ @x +b @ @y : (3.42) 97 SinceD q =S q H q ; we haveH q = span n @ @ ; @ @ o : Finally, the Lagrangian of the system isL(q; _ q) = 1 2 _ q T M _ q where M = 2 6 6 6 6 6 6 6 6 6 6 6 6 6 6 4 I 0 I 0 0 0 2J 0 0 0 I 0 ml 2 0 0 0 0 0 m 0 0 0 0 0 m 3 7 7 7 7 7 7 7 7 7 7 7 7 7 7 5 : The reduced Lagrangian is, therefore: `(r;u;) = (u; ) T M (u; ): There is only one direction along which snakeboard motions lead to momentum conservation: it is dened by the basis vector e 1 (r) = 2l cos 2 2 6 6 6 6 6 6 4 tan l 1 0 3 7 7 7 7 7 7 5 ; and, hence, there is only one momentum variable 1 = @` @ ;e 1 (r) . Using this variable we can derive the connection according to [45, 2] as [A] = 2 6 6 6 6 6 6 4 I ml 2 sin 2 0 I 2ml sin 2 0 0 0 3 7 7 7 7 7 7 5 ; and = 1 4ml 2 cos 2 e 1 (r): Continuous Equations of Motion The dynamics of the system can be derived either in terms of or in terms of as unknown variables. Here, we provide the resulting 98 equations of motion based on since this has been the choice in previous work and will be easier to compare against. The continuous equations of motion (Sec. 3.2.3.2) can be derived as 2 6 6 6 6 6 6 4 _ x _ y _ 3 7 7 7 7 7 7 5 = 2 6 6 6 6 6 6 4 cos sin 0 sin cos 0 0 0 1 3 7 7 7 7 7 7 5 0 B B B B B B @ 1 2ml ( 1 sin 2 _ ) 2 6 6 6 6 6 6 4 1 0 tan l 3 7 7 7 7 7 7 5 1 C C C C C C A ; (3.43) _ 1 = 2I cos 2 _ _ 1 tan _ ; (3.44) 1 I ml 2 sin 2 = I 2ml 2 sin 2 _ _ 1 2ml 2 _ p + 1 I f ; (3.45) = 1 2J f : (3.46) A nonholonomic integrator The discrete equations of motion will be derived by sub- stituting the Lagrangian and the connection of the snakeboard into (3.40). It seems more convenient from implementation point of view to express the Euler-Lagrange equations of the base dynamics using the reduced Lagrangian ` rather than the constrained reduced Lagrangian l c so we use (3.40) instead of (3.39). We also need to choose the map and in the remainder of this section we set = exp, i.e. the exponential map. The expressionhDEP exp (k);e b (r k )i involves the term dexp 1 dened in (2.13). Ob- serving that in the case of the snakeboardhad ;i = 0 for any 2 h and ;2 s and therefore h(dexp 1 ) ;e 1 (r)i =h;e 1 (r)i 99 and the operator DEP exp simplies to hDEP exp (k);e 1 (r k )i :=h k k1 ;e 1 (r k )i Note that sincehDEP exp (k);e 1 (r k )i = 0 thenhDEP exp (k);A(r k )i = 0 sinceA(r k )2s r . The equations of motion become g 1 k g k+1 = exp(h( k A(r k+ )u k )); r k+1 r k =hu k ; k = @l k+ @ ; h k k1 ;e 1 (r k )i = 0; @` k+ @u @` k1+ @u =h (f k1+ + (1)f k+ ) for k = 1;:::;N 1. Dening = A(r)u and u = (u ;u ) the equations are derived by substituting = (ml 2 1 +Iu ;m 2 ; 0); @` @u = (I(u + 1 ); 2Ju ): Numerical Comparisons Our numerical comparisons for nonholonomic motions are based on one-minute trajectories of the snakeboard. The vehicle is controlled using si- nusoidal inputs of frequency and amplitude designed to produce nontrivial paths such as parallel parking, sharp turns, and winding maneuvers. Since the trajectory is relatively short all RK methods up to fourth order that we test (Fig. 3.6) are stable due in part 100 10 2 10 3 10 4 10 −8 10 −7 10 −6 10 −5 10 −4 10 −3 10 −2 10 −1 10 0 timesteps meters Average Position Error Ours Eul RK2 RK4 10 2 10 3 10 4 10 −8 10 −7 10 −6 10 −5 10 −4 10 −3 10 −2 10 −1 timesteps rad. Average Rotation Error (θ) Ours Eul RK2 RK4 10 2 10 3 10 4 10 −8 10 −7 10 −6 10 −5 10 −4 10 −3 10 −2 10 −1 10 0 timesteps rad. Average ψ Error (θ) Ours Eul RK2 RK4 10 2 10 3 10 4 10 −8 10 −7 10 −6 10 −5 10 −4 10 −3 10 −2 timesteps rad. Average φ Error (θ) Ours Eul RK2 RK4 0 0.2 0.4 0.6 0.8 1 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1 10 2 10 3 10 4 0.5 1 1.5 2 2.5 3 3.5 timesteps microsec ~ 10 −6 sec Average Runtime per Update Ours Eul RK2 RK4 Figure 3.6: Stability and eciency of our nonholonomic integrator for snakeboard tra- jectories: averaged over 50 runs using a large range of initial conditions and steering commands, our nonholonomic integrator remains as accurate as RK2, at a fraction of the computational complexity. to the simpler group structure of SE(2). Yet, our integrator performs better RK2, at a fraction of the computational time|due to its explicit update scheme. 101 Chapter 4 Global Motion Planning 4.1 Introduction There are two main disadvantages in using optimal control methods in practical appli- cations directly. First, if numerous complicated nonlinear constraints were imposed then nding a feasible solution would be extremely dicult and costly. Second, even if a fea- sible solution is found it is in general only locally optimal and there is no way of knowing how \good" it is unless there is specic prior knowledge. In this chapter we propose a way to overcome these limitations by combining DMOC with a family of methods known as sampling-based roadmaps in order to eciently compute near globally optimal solution trajectories even in very complex scenarios. The main idea is to precompute many simple and versatile optimal motions using DMOC oine, that can then be combined into longer more complicated trajectories that can accomplish the given task. Furthermore, these motions can be combined to form not only a single trajectory but also a tree or a graph whose edges are the motions. This graph can be expanded towards parts of the space that might lead to more optimal trajectories 102 and it would also contain edges reaching the goal state{therefore, such a graph, termed roadmap, contains many possible trajectories to the goal. Finding the optimal trajectory then amounts to nding the shortest path in the graph. Thus, a roadmap representation has two main advantages: it provides a compact way to encode many dierent paths from the start state; it lends itself to well-established dynamic programming methods for nding an optimal solution satisfying the dynamics by construction. In essence, the roadmap framework can be used to transform a very high-dimensional dierential problem into a lower dimensional algebraic problem (guring out how to sequence simple motions) and graph search problem (nding the best path). The solution is as good as the approximation{the bigger and denser the space the roadmap covers and the richer the set of primitive motions used, the more optimal the solution would be. Fortunately, this is a very active research area in robotics and various strategies exist for constructing roadmaps in order to solve motion planning problems with many degrees of freedom, from coordinating multiple humanoid robots in urban environments to docking molecules in protein design. Challenges When the system dynamics is nonlinear or when the state space has com- plicated boundary and is non-simply connected, e.g. arising from joint limits, environ- ment obstacles or velocity bounds, then there would be many locally optimal solution trajectories. For instance, even in the absence of physical obstacles, the computation of minimum-acceleration trajectory for a non-spherical rigid-body could lead to multiple locally optimal solutions based on how the variational method was initialized. This is be- cause the necessary conditions for optimality are not ane and could have multiple roots. 103 When obstacle are present and must be avoided the situation becomes more complicated since they immediately induce multiple homotopy classes of paths in the conguration space and an even more complicated topology of the state space (the tangent bundle) when velocity limits are present. The importance of exploration The only way to guarantee that a trajectory is op- timal is to generate trajectories that 1) visit every region in the reachable state space, and 2) then reach the goal state. This would guarantee that all possible routes to the state goal have been taken and the best one chosen. Since it is impossible to produce such innite number of trajectories a realistic approach would be to produce trajectories that visit only a subset of the state space that is likely to lead to good solutions. This gives rise to a trade-o known as exploration vs. exploitation known in robotics and rein- forcement learning, where exploration refers to controlling trajectories to visit unexplored but potentially leading to a more optimal solution regions, while exploitation amounts to then guiding this trajectory to achieving the goal or task. Often, a complicated state space would require more extensive exploration while a simple motion planning problem can be solved solely with exploitation (e.g. the simple case of stabilizing a linear system without obstacles is a convex optimal control problem that does not need exploration). Related Work Sampling-based motion planning has been a very active research area during the past decade. Good summary of current methods can be found in the recent books [35, 11]. Multiple research groups focus on the development of new sampling-based techniques motivated by wide variety of applications not only in robotics, but also in areas such as molecular biology and graphics. More prominent research teams include 104 the group lead by Latombe at Stanford, Kavraki at Rice, LaValle at the University of Illinois at Urbana-Champagne, Amato at Texas AM, and others. These researchers have developed various methods and techniques applicable to a broad spectrum of examples, from planning for multiple digital actors in virtual worlds to computing optimal docking maneuvers for complex protein structures. A full review of the planning literature is beyond the scope of this thesis and in the following sections we would only brie y describe the methods applicable to the particular problems considered. 4.2 Sampling-based Roadmaps Rapidly-exploring random tree incremental roadmap Figure 4.1: A simple example: nding a trajectory from the start (lower-left corner) to a goal state inside the \bug-trap". A rapidly-exploring random tree (RRT) is used (left) to quickly explore the environment and nd any path (usually far from optimal). Once a path is found using RRT, the expansion switches to an incremental roadmap method (right) that focuses on nding a more optimal solution. The term roadmap can be associated with several dierent underlying representation based on the type of system and motion planning task required. Its most widely used 105 representation is an undirected graph which is applicable to kinematic systems without dierential constraints and oers the ability to nd a motion between any two congu- rations in the graph [35]. Methods in this category are called multiple-query probabilistic roadmaps (PRM). When computing a motion plan for a mechanical systems with dier- ential constraints a roadmap is constructed as a directed graph or a tree rooted at the starting state. Dierent strategies exists for guiding the roadmap expansion. A family of methods called rapidly-exploring dense trees (RDT) which include the more well known rapidly-exploring random trees (RRT) are designed to quickly explore the state space to nd any feasible path to the goal (see Fig.. There is no notion of optimality in these methods since they involve cost metrics aimed at fast expansion rather than considering the full cost of achieving the motion task. In contrast, incremental PRM methods for mechanical systems have also been proposed (e.g. [21]) that are similar to RDT but focus on reaching the goal while minimizing a trajectory cost metric and producing optimal trajectories. In general, the RDT method nds any feasibly path quickly, while the PRM approach nds a more optimal path with more computational eort. Since we are inter- ested in motion planning for mechanical systems under dierential constraints, we use the term \PRM" (possibly con icting with terminology used by other authors) to indi- cate incremental planners that also include the RDT types. In fact, similar to [21] our implementations use a hybrid method between RDT and single-query PRM that initially focuses solely on exploring the space and then interleaves exploration with narrowing down a near-optimal solution. 106 Problems of Interest There are two types of problems that we are interested in. The rst are typical motion planning problems of moving from a given start state to a goal state minimizing a given cost function dened as a metric on the state space. The second are problems of nding a trajectory that minimizes a more general cost function subject to constraints but does not need to end at a predened goal state. An example of the second kind is computing a trajectory that achieves maximum sensor coverage of the environment subject within a given time. The second type of problems are much harder since the cost function often depends on the whole trajectory and is not directly related to a metric on the original state space. Our approach to achieving global exploration of the state space is based on the RRT model [35] and the incremental PRM method [21]. The underlying structure is a treeT with nodesN and edgesE. A node is denoted n2N and an edge between two nodes n a and n b is denoted e (a;b) 2E. Random tree construction The RRT exploration algorithm is given in Fig. 4.2. The RRT expands by sampling a new node from the state space, nding the closest existing node in the tree, and then extending the tree from that node towards the sampled node. Either a partial or complete (reaching the node) trajectory can be produced based on which strategy is used. Incremental roadmap construction The incremental roadmap planning algorithm is depicted in Fig. 4.2. The algorithm is given a root node n r and goal node n g containing the initial and nal desired states. A given minimum desired trajectory costc des serves as a termination condition. At every iteration a new node n s is sampled from the state space. 107 RRTExplore(n r ) N =fn r g WHILE (! Terminate(N )) n s := Sample() (sample new node) n b := NearestNeighbor(N; n s ) (nd nearest neighbor from tree) e (b;s 0 ) := Extend(n b ; n s ) (extend it towards sample) IF e (b;s 0 ) (if successfully extended) N :=N[ n s 0 E :=E[ e (b;s 0 ) Figure 4.2: Rapidly-exploring random tree (RRT) exploration pseudocode The tree expansion relies on a distance function between the nodes. The ideal distance function is the same of the objective function to be minimized. Since newly sampled nodes are not yet connected to the tree, there is no way of knowing what this cost is and therefore a heuristic distance is used that underestimates the true distance. All existing nodes are then sorted using this cost in an arrayN b . The tree is then extended from the best node (with lowest cost) in this array n b to the sample. The function Extend relies on a local planner that usually reaches the sample node n s only approximately resulting in a new node n s 0. If the new resulting cost at n s 0 has a chance of improving the current best cost c best then the edge is added. Then, an attempt is made to reach the goal from the newly added node. If this succeeds and the best cost is improved, then the tree can be pruned and the algorithm continues. Local planner Local planning between nodes can be accomplished in several dierent ways. In some cases, e.g. when considering unconstrained kinematic systems, a trajec- tory can be computed simply using interpolation. A more general method applicable to systems with dynamics or underactuation is to use a controller that stabilizes the system 108 PrmPlan(n r ; n g ;c des ) N =fn r g c best =1 WHILE (c best >c des ) n s = Sample() (sample new node) N b = Sort(N; n s ) (sort by distance to n s ) FOR i = 1 : size(N b ) n b =N b (i) e (b;s 0 ) = Extend(n b ; n s ) (extend towards sample) IF e (b;s 0 ) AND ImproveCost(e (b;s 0 ) ) (if cost can be improved) N =N[ n s 0; E =E[ e (b;s 0 ) (add node to roadmap) e (s 0 ;g 0 ) = Extend(n s 0; n g ) (extend towards goal) IF e (s 0 ;g 0 ) AND ImproveCost(e (s 0 ;g 0 ) ) N =N[ n g 0; E =E[ e (s 0 ;g 0 ) c best = cost-to-come at n g 0 (update best cost) Prune(N ) (prune nodes with higher cost) Figure 4.3: Incremental probabilistic roadmap (PRM) planning pseudocode to the state of the new node. A drawback in such an approach is that the resulting path could be far from optimal and that often times it is necessary to stabilize to non-zero velocity states which raises issues of stability. A third approach is to use optimal control and nonlinear programming for computing an optimal trajectory. The drawback of that approach is that its convergence is very sensitive to the choice of initial guess and the computation is extremely costly. An alternative approach that we rely on in this work is to use a set of simple precomputed optimal control motions called primitives in order to construct more complex, suboptimal trajectories that approximately achieve the local plan very eciently. 109 4.3 Planning using primitives 4.3.1 Primitives In this section we recall the formulation of primitives for motion planning of dynamical systems introduced in [22]. Assume that a control systemS is dened on a state-spaceX and has a set of allowable controls inputs denoted U. Usually for mechanical systems X is the tangent or cotangent bundle,TQ orT Q respectively, whereQ is the conguration space. Let x2X denote the state, u2U the controls. A trajectory from timet i to timet f is denoted :t2 [t i ;t f ]! (x(t);u(t)). Planning using primitives relies on the notion of trajectory invariance. Assume that the Lie group G acts on X with action on x denoted (g;x), where g2G. An invariant trajectory is such that the dynamics of the system is G-invariant. More formally, denoting the ow (or integral curve) of the system by' :XR!X then the invariant ow from the start state x 0 2X is '(x 0 ;t) satises g ('(x 0 ;t)) ='( g (x 0 );t): Two trajectories 1 and 2 are equivalent if they dier by a group action and a time translation, i.e. if there exists a g2G and a time T such that (x 1 (t);u 1 (t)) = ( g (x 2 (tT ));u 2 (tT ));8t2 [t i;1 ;t f;1 ] The concatenation of two trajectories requires the denition of compatibility. Two trajectories 1 and 2 are compatible, denoted 1 C 2 , if there exists g 12 2 G s.t. 110 1 2 3 1 2 3 Figure 4.4: Examples of optimal helicopter primitives (computed using DMOC) invariant with respect to the action of group G 0 = SO(2)R 3 . The top three trajectories are trim primitives of constant forward motions with translational velocity v = 15m/s and rotational velocity ! = 0 /s ( 1 ); v = 15m/s and ! = 30 /s ( 2 ); v = 10m/s and ! = 15 /s ( 3 ). The bottom three trajectories are maneuvers that start from rest and achievev = 15m/s and! = 0 /s ( 1 ); start withv = 15m/s and! = 0 /s and stop at rest ( 2 ); start from rest and achieve v = 15m/s and ! = 30 /s ( 3 ). Various concatenations of these primitives are then possible, e.g. 2 C 1 C 3 or 1 C 2 . x 1 (T 1 ) = (g 12 ;x 2 (0)) Any invariant trajectory can be regarded as a primitive. The set of all G-invariant primitives is denotedP(S;G) and is closed [22]. Therefore, new more complicated primitives can be constructed by concatenating simpler ones as long as they are compatible. Fig. 4.4 shows examples of helicopter primitives. There are two main classes of primitives useful for motion planning. They are dis- cussed next. 111 4.3.1.1 Trim Primitives Trim trajectories correspond to continuously parametrized steady-state motions. For- mally, :t2 [0;T ]! (x (t);u a (t)) is a trim primitive if: x (t) = (exp(t );x (0));u (t) =u ;8t2 [0;T ] or if the motion is a nite ow along left invariant vector elds with constant control inputs. The set of trim primitives is denotedT (S;G). Once a trim primitive vector eld has been identied it can be used to construct a one-parameter family of trim primitives () :t2 [0;T ]! ((exp(t );x (0));u ); where is called coasting time, and the set of all such primitives dened by T = f(); 0g. The displacement of a trim primitive with coasting time is simplyg = exp( ). 4.3.1.2 Maneuvers Maneuvers are designed to eciently switch from one steady-state motion to another. Therefore they are dened to be compatible form left and right with trim primitives. The set of maneuvers is denotedM(S;G)P(S;G). Formally, a maneuver satises 2M(S;G),9;2T (S;G) :2P(S;G): 112 The displacement as a result of executing the maneuver is denoted g . 4.3.2 Computing motion plans The motion planning problem can now be solved by nding a sequence of primitives! con- sisting of trim primitives 1 ;:::; N+1 , with coasting times = ( 1 ;:::; N+1 ), connected by maneuvers 1 ;:::; N . The pair (primitives, coasting times) is denoted (!;)2P(S;G) and is dened by (!;) = 1 ( 1 ) 1 2 ( 2 ) 2 ::: N N+1 ( N+1 ) The task is to nd (!;) that takes the system from an initial state x i 2 X to a nal goal state x f 2X or to a set X f X. Controllability Although by assumption the original control system is controllable, a system that is forced to move only along primitives might not be controllable to an arbitrary state x f 2 X. Intuitively, this limitation arises since the trim trajectories are of particular form that could constrain the reachable space around x f . Fortunately, the fact that the trim primitives are continuously parametrized makes it possible to consider innitesimal motions by perturbing their coasting times near the origin. The composition of the resulting innitesimal ows can generate new directions of motion to increase the dimension of the reachable set around x f . Similar to the Lie algebra rank condition (LARC) for nonintegrable systems, if the number of the linearly independent iterated Lie brackets of the trim vector elds i equals the dimension of the lie algebra g, then the system is controllable to the conguration of x f [22]. 113 The total group displacement after executing trajectory p = (!;) would be g p = " N Y i=1 exp( i i )g 1 # exp( N+1 N+1 ) Therefore, as the authors of [22] point out, the original dierential control system was transformed into a purely kinematic system, with the transformation being exact without any approximations. It is reasonable to assume that the initial and nal conditions are given in terms of steady states (or trim primitives) x (0) and x (0) respectively, i.e. there are group elements g 0 ;g f such that x 0 = (g 0 ;x (0)) and x f = (g f ;x (0)). Then computing a motion from x 0 to x f amounts to nding a motion plan p = (!;) such that g p =g 1 0 g f Solving this equation is equivalent to the problem of kinematic inversion in robotics. Furthermore, any cost function to be minimized can also be reexpressed in terms of cost of executing the individual primitives. The resulting optimal control problem is solved algebraically without the need of nite-dierence approximation of the original control system derivatives. 4.4 Motion Planning using DMOC and Roadmaps The main idea is to start by computing many simple short but optimal motions through DMOC optimization. These motions are guaranteed to be optimal since they are selected 114 as the best solution from multiple optimization runs with varying initial conditions. The trajectories are then used to build a library of primitives. A cost function minimizing motion plan can then be formulated as a kinematic optimal control problem as described in Sec. 4.3.2. A feasible solution to this problem might still be dicult to compute and far from optimal when that state-space is complex. Therefore, a global dynamic- programming approach such as a sampling-based roadmap would be appropriate and is explored in this section. 4.4.1 Trajectory Discretization Time Discretization There are dierent types of DMOC discretization depending on the type of systems studied. This results into dierent ways of dening the state space for roadmap planning using primitives: The case of vehicles on Lie groups (2.4) is dened using a discrete state (g k ; k ; k )2 Ggg where k is computed from k using the Legendre transform. The state space is X :=Gg; x k := (g k ; k ): The more general case of vehicles with symmetries and nonholonomic constraints (3.24) is dened using a discrete state ((r k ;u k ;p k ); (g k ; k ; k ))2 (TMT M) G s g where p k and k are computed from u k and k using the Legendre transform and k = k A(r k +hu k )u k . The minimal state is X :=TMGs; x k := (r k ;u k ;g k ; k ): 115 Discrete Evolution The evolution of the state in DMOC is dened using the discrete ow ' d :XN!X (x k ;i)!x k+i ; i 0 The discrete ow is computed recursively using an integrator. The integrator can be either explicit, of the general form: x k =f(x 0:k1 ;u 0:k1 ); or implicit, taking the form f(x 0:k ;u 0:k ) = 0; wherex 0:k denotes the sequence of statesx 0 ;:::;x k . Normally, when rst order discretiza- tions are used,f only depends on the last statex k1 and last controlu k1 . Higher order discretization schemes, though, result in dependency on previous states and hence, for generality, f is dened to depend on the whole trajectory and control history. In particular, the following integrators are used for time-stepping update x k !x k+1 based on the type of system: systems on Lie groups: (2.7)-(2.9) nonholonomic systems with symmetries: (3.39) 116 The discrete ow approximates the continuous ow ' : XR! X (see Sec.4.3.1), i.e. ' d (x k ;i)'(x(kh);ih); x k x(kh) Using this relationship the notions of invariance, equivalence, and compatibility dened in Sec. 4.3.1 extend trivially to DMOC trajectories when restricted to their discrete set of states. 4.4.2 Sequencing Primitives Naive Sequencing The sampling based planners rely on a function Extend that must eciently produce a trajectory connecting two nodes. In the primitives planning frame- work this amounts to rst selecting a sequence of trim primitives and maneuvers. Such a selection is a combinatorial problem which can be combined with an optimization prob- lem (usually ane) in order to determine optimal coasting times for trim primitives (Sec. 4.3.2). Although the resulting numerical problem is much easier than the original optimal control problem it still requires combinatorial search and iterative optimization. Instead, we use a simple sequencing strategy by replacing the setT of continuously parametrized trim primitives along some vector eld by the discrete setf( i min )g c i=1 , i.e. by trim primitives of exponentially increasing times. The smallest coasting time min is chosen so thatkg ( min ) k < d min , wherekk : G! R is distance function determin- ing how close two congurations are and d min is a minimum allowable error tolerance on achieving a motion plan. The maximum exponent c is chosen so that c min does not exceed the reasonable time in which we believe the while motion can be achieved. Such 117 an approximation is chosen since it allows the quick construction of a trim primitive with coasting time equal to any factor of min . In essence, the optimization part of the problem is one more time replaced by a combinatorial one at the expense of slight loss of controllability. Sequencing primitives then proceeds in the most simplistic and greedy manner: go through all primitives compatible with the current one select the one that takes the vehicle closest to goal keep iterating until distance to goal is less than d min . This is by no means optimal but it is very ecient, and since our roadmap has the ability to prune bad subtrees then, in practice, bad sequences are quickly replaced by better ones. The constantly decreasing upper bound on the current optimal cost guarantees that only cost improving sequences are considered. Proper Sequencing Proper sequencing involves selecting a set of primitives and coast- ing times that would result in a more optimal trajectory that reaches the sampled node exactly. Such a solution can be computed using nonlinear programming since in general it has no closed-form. We choose to use the naive strategy explained above in order to avoid the extra cost of iterative optimization. In addition, the automatic cost bounding and trajectory pruning at the higher level roadmap construction assures that the resulting path is nearly optimal even when using a naive local strategy. In addition, for for certain simpler problems, it is worth exploring exact inverse kinematics solutions. 118 4.4.3 Examples Helicopter We apply the roadmap planning with primitives to the helicopter model introduced in Sec. 2.10.2. One can view this model as an idealized version of the systems used in [22] with the main dierence that we have chosen an exact form of control in- put vector elds instead of using a control input transformation determined empirically. While not as closely realistic as the form used in [22] our formulation preserves the high underactuation of the system and allows us to show how to compute requirements for trim primitives in closed form. The helicopter dynamics is invariant under transformations by the groupG 0 = SO(2) R 3 , i.e. rotations around z-axis and translations. The primitives that we use are in the form of the vector eld 2se(3) = 2 6 6 6 6 6 6 6 6 6 6 4 0 ! z 0 v x ! z 0 0 v y 0 0 0 v z 0 0 0 0 3 7 7 7 7 7 7 7 7 7 7 5 ; 119 where the angular velocity (around the body-xed z-axis) ! z and the linear velocity v = (v x ;v y ;v z )2 R 3 are chosen in a special way to preserve the dynamics invariance. Based on our model, the conditions for invariance can be computed as: rotor blade angles: p = 0; r = 0 rudder input: u = 0 The remaining variables satisfy: v y ! z = g sin v x ! z = g cos sin u c = g cos cos; where g = 9:81 from gravity, and are the pitch and the roll of the helicopter body computed from its rotation matrix R 2 SO(3). These conditions ensure a constant velocity, i.e. _ = 0 along the whole trajectory. A general primitive with constant velocity in the form can be then constructed by setting: = arcsin v y ! z g ; subject to jv y ! z j<g; = arcsin v x ! z g cos ; subject to jv x ! z j<g cos u c =g cos cos: In other words once v and ! z are chosen then invariance is maintained by requiring that the starting conguration of the primitive has pitch and roll (and hence any conguration along the primitive since these two variables would remain invariant), that 120 the blade angles and the rudder input be set to zero, while the collective u c is held constant as dened above. For example, a trim primitive corresponding to sharp right turn with velocities v = (15; 0; 0) in m/s and ! z = 30 =s requires that = 0, =53:19 , u c = 5:89 N. A less aggressive turn with v x = 10 m/s and! z = 15 /s requires =15:48 andu c = 9:45 N. See Fig. 4.4 for examples of some of these primitives. Our analytical derivation of the invariance conditions is consistent with the experi- mentally determined primitives used in [22] for real helicopter. Similar to this work we can also include non-zero sideways velocity v y and hence non-zero pitch in the primi- tives construction but that is not required since our simple model does not account for air resistance and constant forward velocity can be maintained without pitching forward. The vertical componentv z can be chosen freely within the vehicle envelope and does not aect invariance. Fig. 4.5 shows the constructed roadmap and gives a closer view of the resulting least cost trajectory. One of the nice feature of the sampling-based methods is the ability to quickly explore the state space and nd any solution, which initially is far from opti- mal. The algorithm can then quickly improve that solution as the roadmap covers the environment more densely. Car In the car example (with dynamics described in Sec. 3.4), trim primitives consist of straight forward and backward motions of constant velocity; as well as left and right, forward and reverse turns with constant steering angle and constant velocity. Maneuvers are solutions to optimal control problems with boundary conditions compatible with a 121 top view side view close-up view nal maneuver Figure 4.5: An example of planning using DMOC primitives and an incremental roadmap global search. A helicopter must traverse a cluttered urban environment and arrive inside the cylinder with minimum fuel. The dierent views show the resulting roadmap and least cost trajectory to goal after 1 second of computation. 122 Figure 4.6: Incremental PRM for a car-like robot. The goal is to compute a trajectory that parks the car inside the -shaped parking structure. chosen pair of trim primitives. Fig. 4.6 shows an example of optimal motion of a car into a parking structure. For simplicity, in this example the distance metric is the standard euclidean metric on the (x;y)-position subspace. 4.5 Extensions In this section we consider several important extensions to the general motion planning framework. For simplicity, the simulation results are based on vehicles with simple uncon- strained dynamics subject to velocity limits. Since the roadmap method is not specic to any particular dynamics model the developed algorithms are applicable to vehicles with realistic dynamics as well. 123 4.5.1 Moving Goal State Assume that the goal state is moving in time and the vehicle has complete knowledge of its dynamical model. For clarity denote the goal state at the k-th time stage by y k 2X. The task is still the same: to reach the goal state with minimum cost. For computational purposes, the trajectory of the goal can be precomputed up to the time horizon of K stages as y 0:K . The roadmap algorithm can be used with slight modication when extending the roadmap towards the goal. Let the time and state of the roadmap node to be extended by denoted by (t k ;x k ). In order to connect this node to the goal trajectory one can simply sample an integer l, k < l < K, and extend towards the pair (t l ;y l ). Depending on the cost function, it might be possible to choose a heuristic for better sampling. For example, if the cost function is minimum time, then l can be chosen such that (lk) time steps is a lower bound on time required to move from x l toy l , and if such extension fails to continue with increasingl until the minimuml is found. Fig. 4.7 shows results for reaching the goal in minimum time using a point-mass vehicle with limits on the velocity. 4.5.2 Optimal Coverage Consider the case of a vehicle with circular sensing area of xed radius. Obstacles in the environment restrict the sensor coverage, e.g. the sensor such as an omnidirectional camera covers only the physically visible area. The task is to compute a vehicle path that maximizes the union of all area covered within some prescribed time horizon K. This is a very dicult, computationally intractable problem. It has been shown that it can be reduced to a high-dimensional traveling salesman problem (TSP) which itself is 124 t f = 65 s. t f = 45 s. t f = 39 s. closer view Figure 4.7: Finding a time optimal trajectory to reach a time-varying goal state with known dynamics. The goal state is the lower right at time t = 0 and starts to move north going around obstacles{its projected trajectory is drawn leaving the environment at time t = 100 s. Consecutive stages of the roadmap expansion show how the current best solution improves (the cost function is the time t f of reaching the goal ). 125 NP-hard and at best has exponential complexity if solved through dynamic programming. Our goal is to compute a good (but not optimal) solution through the roadmap framework (which in essence combines dynamic programming with randomization). Applying the roadmap approach is straightforward. The only modication is to the cost function. The heuristic cost used during expansion is based on assuming there are no obstacles and can be computed quickly. The actual cost is then computed using standard computational geometry methods for polygon area (in 2-D) and polyhedra volume(in 3-D) and intersections. In particular the cost used is the total area covered divided by the total trajectory time, i.e. the coverage rate. Denote the workspace to be covered byW. Depending on the sensing applicationWR 2 orWR 3 . Let the vehicle cover a regionA(x k )W while it is at state x k at time t k . The cost function at x k is then cost(x 0:k ) = area(A(x 0 )[:::[A(x k )) t k t 0 It is evident that the change in the cost from one node to the next depends on the whole trajectory and not only on the new segment. Therefore, in the standard planning framework a system state at timet k must be not the statex k but the whole trajectory up to that timex 0:k . Only then the cost function is a distance metric satisfying the triangle inequality and useful for dynamic programming in a graph structure. Since constructing and using such states is impractical we only consider a tree structure in which nodes can still be described by the original states x k given that the cost is recomputed after the 126 trajectory is extended. This can be done eciently by storing the region covered by the whole trajectory at each node. See Fig. 4.8. Figure 4.8: A vehicle that can sense everything outside the obstacles up to a xed radius. Maximizing the total area searched (shaded) in xed time horizon of of 100s. Consecu- tive stages of the roadmap expansion show how the current best solution improves (the coverage cost is the total shaded area shown). 4.5.3 Uncertain Target Detection with Multiple Vehicles Imagine that some interesting phenomenon is taking place in the environment. Assume that we have multiple vehicles equipped with sensors that can make noisy measurements of this phenomenon. For example, this phenomenon could be a single object of interest 127 that is moving in a cluttered environment with some uncertainty, e.g. we might know that it is heading north with speed around 5 m/s and that it might slow down, avoid obstacles, speed up, etc... In this example we consider the problem of vehicle deployment in order to maximize the information gained about the target. In this section we only brie y describe some experimental results without describing the probabilistic framework in detail. For instance, Fig. 4.9 gives a few consecutive snapshots of the vehicle and target states during deployment. Figure 4.9: An example scenario: a target with uncertain dynamics is moving north avoiding obstacles. Its possible motion is represented by a nite set of particle trajectories. These trajectories are used to simulate future measurements taken by the vehicles. Two vehicles with circular sensing eld-of-view (with initial position in the lower left corner) are deployed in order to maximize the probability of detecting the target. In order to solve the problem we use the RRT exploration algorithm (Fig. 4.2). A node contains the states of all vehicles and a probabilistic estimate of the target state computed from sensor measurements. Since there is no preexisting target motion the measurements must be simulated. Since the target is moving whole trajectories of measurements must be simulated. Furthermore, we represent the uncertainty in target motion by generating 128 multiple target trajectories (and hence use multiple target trajectories measurements) in a Monte-Carlo fashion. Consequently, any measure of uncertainty or information is computed by taking its expected value over these possible target motions. Therefore, each node stores a set ofN probabilistic lters (whereN is the number of Monte-Carlo target trajectories) each lter updated by assuming that the true target motion is a particular trajectory from that set. In our implementation we use Kalman lters of constant velocity and white noise acceleration based on relative position measurements assuming Gaussian error sensor model. A proper implementation would also involve importance resampling of the Kalman lter states in order to avoid estimates with very low probability. We leave that for future work. The distance metric used for planning is a combination of time and the inverse of the expected variance (i.e. the norm of the lter covariance) of the target state estimate. The tree expansion is designed to achieve global exploration while also attempting to connect with areas with high expected target probability mass. This is achieved by drawing a small fraction of the tree samples from the actual simulated measurement particle set treated as a probability distribution. A near-optimal solution is depicted on Fig. 4.10. 4.6 Conclusion This section presented a way to combine optimal control and global state space search in order to solve the problem of deploying one or multiple vehicles to achieve a certain 129 initial conditions least cost vehicles trajectories Figure 4.10: A single target with time-varying uncertain dynamics (with distribution in time represented by particle trajectories) must be detection with maximum probability and in minimum time. Two vehicles with circular sensing eld-of-view and limited velocity are deployed based on sampling-based roadmap computation. goal such as reaching a goal state or minimizing a task specic cost function. Our ap- proach is based on combining DMOC with motion planning methods developed in the robotics community and extending the framework to interesting problems involving real dynamics, uncertainty, and complex cost metrics. Some of the resulting algorithms (such as maximizing coverage from Sec. 4.5.2) are not guaranteed to be near optimal because of the exploding dimension of the search space inherent to the specic problem. Yet, even in such dicult problems employing a roadmap representation build from optimal primitives proves useful since it encodes many possible motions in a compact and ecient manner. Proper treatment of uncertainty in our framework is still an open problem. An exam- ple of deployment with uncertain target dynamics and simple noisy sensing was given in Sec. 4.5.3. Some initial progress has been made in incorporating uncertainty in more real- istic dynamical models in order to achieve the task at hand while minimizing the chance 130 Figure 4.11: Uncertainty in the helicopter position. Dierent motions resulting from uncertain external disturbance forces, e.g. arising from wind. The trajectories and the resulting position uncertainty ellipsoid are generated by sampling from a Gaussian exter- nal force error model. shortest path safest path Figure 4.12: A helicopter with uncertain dynamics (as shown in Fig. 4.11) must y to a goal location across a digital terrain. Its position estimate uncertainty norm is drawn as a tube along the path. It has no GPS and can only measure its own position by observing static beacons in the environment. The path on the left shows the shortest path to the goal but clearly demonstrates that the resulting uncertainty would increase the chance of collision with the terrain. The path on the right is aimed at minimizing the chance of collision by minimizing the position uncertainty along the path in the vicinity of obstacles. 131 of violating certain constraints such as colliding with obstacles. An example of ongoing work with a helicopter model (described in Sec. 2.10.2) with uncertainty in control inputs and external disturbances is depicted in Fig. 4.11. A computed scenario inspired by the related work [49] is described in Fig. 4.12. The main challenges in incorporating uncertainty lie in choosing a numerical repre- sentation that would result in a balance between accurate noise propagation and ecient computation suitable for the kinds of deployment problems discussed in this section. Ulti- mately, one needs to examine and deal with sources of uncertainty not only in the vehicle sensing and actuation but also in connectivity and communication that are extremely important in multi-vehicle and distribution applications. 132 Chapter 5 Homotopy Continuation of Motion Planning Constraints 5.1 Introduction The goal of this chapter is to apply homotopy continuation methods to motion planning of mechanical systems such as autonomous robots that are subject to complex constraints. Such constraints can arise either from the environment, such as obstacles; from the dy- namics of the system, such as nonholonomic constraints or underactuation; or from the given task, such as maintaining visibility to a target of interest during motion. The main idea behind the homotopy methods that we propose is to initially solve an easier (usu- ally trivial) problem and then continuously deform it into the original problem arriving at the true solution. This deformation could have physical meaning such as shrinking or smoothing of obstacles, or gradually adding/removing degrees of freedom of motion, or could be a based on nonphysical numerical homotopy such as convexication of the equality relation describing the constraint. The advantage of the homotopy approach is that one can start with an initial solution that is easy to compute and repeatedly solve a slightly more complex problem using the solution of the previous one as an initial 133 guess until the original problem is solved. If slight variations of the problem result in slight variations of the solutions, then the method oers a stable and ecient way to solve otherwise complicated problems. The theory of homotopy continuation establishes conditions under which such deformation is possible and provides numerical methods for tracing a solution. The problems that we study involve constraints that are described by algebraic equal- ities or inequalities. Such relations are derived, for example, from the discretization of the systems dynamics or from the approximation of obstacle boundaries using algebraic sets. The homotopy methods described here apply to motion planning methods that are based on such constraint representation. Optimal control problems such as minimizing the con- trol eort while satisfying boundary conditions are our main application. Nevertheless, the homotopy approach applies to other control problems such as point stabilization or trajectory tracking as well. Next we present the general concept behind homotopy and describe two standard nu- merical continuation methods that apply to systems evolving on continuous conguration spaces. Then we give several applications: relaxation of environment obstacles through physical and non-physical deformations; smoothing of rough terrain to relax path plan- ning constraints for legged robots; relaxing the underactuation of simple control systems. 134 5.2 Homotopy Continuation We recall the basic formulation of the homotopy method, following [1], applied to the problem of nding a solution to the N nonlinear equations in N unknowns F (x) = 0; where F : R N ! R N is a smooth map. Without prior knowledge of the problem a standard root-nding technique might fail because of poorly chosen initial guess. This can be avoided by using a homotopy (or deformation) H :R N+1 !R N dened by H(x; 1) =G(x); H(x; 0) =F (x); where the smooth map G : R N ! R N has known or easy to compute solution. For example, a commonly used homotopy is H(x;) =G(x) + (1)F (x): The goal is to trace the implicitly dened curve c(s)2 H 1 (0) from the initial solution (x 1 ; 1) to the actual solution ( x; 0). If zero is a regular value of H then such curve exists and is dieomorphic to the real line or the circle. This is equivalent to having the Jacobian DH have full rank for all values ofx and. The curvec(s) is then a submanifold of dimension 1, wheres is usually arc-length parameter. 135 Bifurcation points can occur whenever the Jacobian loses rank. The setH 1 (0) could then have isolated points and branches that need to be handled in special way in order to reach ( x; 0). Such abnormal conditions can be detected whenever the determinant of the augmented Jacobian det 2 6 6 4 DH(c(s)) _ c(s) T 3 7 7 5 becomes zero or changes sign. The goal solution ( x; 0) can be reached from the initial solution (x 1 ; 1) using various methods. The simplest way is to gradually change the value of from 1 to 0 using small decrements and solve the resulting problems. This is called embedding. Often is not the most appropriate parameter for tracing the solution because it might require very small increments to guarantee convergence or the solution curve might have turning points past which embedding cannot continue. In such cases it is more natural to use the arclength parametrization of the combined curve (x;). The family of continuous methods based on such parametrization are called Predictor-Corrector (PC) methods. 5.2.1 Embedding Method The embedding method follows the general scheme 136 Embedding Algorithm Select 8 > > < > > : x 1 2R N such that H(x 1 ; 1) = 0; integer m x =x 1 ; = 1=m for = 1; 1 ;:::; ; 0 solve H(x;) = 0 end (5.1) 5.2.2 Predictor-Corrector Method The PC method is formulated as an initial value problem of integration along the curve c(s) from = 1 to = 0. The problem is obtained, following [1], by dierentiating H(c(s)) = 0 with respect to s, so that the following conditions are satised DH(c)_ c = 0; k_ ck = 1; c(0) = (x 1 ; 1): These sets of equations can be transformed into the ODE c(0) = (x 1 ; 1); _ c =t(c); 137 that can be integrated until = 0 is reached. If DH is nonsingular then the tangent vector t :R N+1 !R N+1 can be easily obtained by QR decomposition of DH since if DH T =Q 2 6 6 4 R 0 T 3 7 7 5 ; then since DHt = 0, we have DH T ;t =Q 2 6 6 4 R 0 0 T 1 3 7 7 5 ; and t can be taken as the last row of the orthonormal matrix Q. The PC method then follows the general scheme: PC Algorithm Select x 1 2R N such that H(x 1 ; 1) = 0; u = (x 1 ; 1) while 6= 0 determine stepsize h u =u +ht(u) (prediction) solve H(u) = 0 (correction) end (5.2) 138 Several issues determine the success of the PC method: selecting the right stepsize h; making sure that t does not change direction accomplished by monitoring the de- terminant of the augmented Jacobian; handling singularities and resulting bifurcations; implementing an ecient corrector solver. Here we have given only the very basic notion behind the method without any of the intricate details. We refer the reader to [1] for further information. 5.3 Applications We apply the homotopy techniques to several optimal control problems. The solution of these problems can be obtained either by direct methods based on sequential quadratic programming (SQP) or indirect methods based on root-nding. In either case we apply the embedding algorithm (5.1) to the resulting system of equations to be solved. We have some preliminary results with PC methods (5.2) as well but we postpone them for future publications. The problems involving the constraints described below are dicult to solve without a good initial guess. Finding a good initial guess is almost as hard as the original problem which motivates the use of homotopy continuation. 5.3.1 Obstacle Constraints Assume that we are given a mechanical system on a conguration space with complex inaccessible regions induced, for example, by physical obstacles in the environment that must be avoided. If the environment is cluttered with obstacles with complex boundaries 139 it is often easier to nd a solution by relaxing the obstacle avoidance requirement. This can be achieved in several ways that we discuss next. In the following subsections assume that the conguration space of the system is Q, and a conguration is denoted q2Q. 5.3.1.1 Distance Function Obstacle avoidance is measured by the distance between the two closest points from the set of points occupied by the mechanical system and the set of points dening the obstacle. Assume that the mechanical system occupies a regionA(q)Q that is a function of its conguration q. The obstacles in the environment are denotedO i Q and the set of all obstacles isO =fO i g: Assume that we are given a distance function :QO!R that computes the distance (q;O i ) between the vehicle at conguration q and obstacleO i . By convention, the distance is positive when the vehicle and the obstacle are not colliding and negative otherwise. The algebraic constraint enforcing obstacle avoidance is then (q;O i )> 0 If the obstacle or the vehicle have complex nonsmooth boundary then the distance func- tion would also be nonsmooth and would aect the convergence of nearby trajectories especially if gradient-based methods such as optimal control or potential eld navigation 140 are used to compute a motion plan. This can be remedied by introducing the following homotopy (q;O i ) =kqq 1 k + (1)(q;O i ); where q 1 2 Q is an appropriately chosen conguration. For example, one physically meaningful choice for q 1 is the centroid of the obstacle. In this case, convexies by enclosing the obstacle in a ball that smooths its boundary while gradually growing it back to its original form as goes from 1 to 0. Example: Helicopter in a digital elevation map Fig. 2.5 shows the trajectory of a simple helicopter that must y optimally in a digital terrain map. The trajectory was computed using the distance function homotopy with q 1 chosen to lie approximately at the center of the tall mountain in the middle. 5.3.1.2 Algebraic Obstacles It is common to represent objects as the intersection of half-spaces dened by smooth algebraic hypersurfaces in Q. For example, registration or parametric shape tting of sensor data \point clouds" in 3-D are standard methods for producing such object repre- sentation. When the surfaces are planes the obstacle regions are polytopes, i.e. polygons in 2-D, polyhedra in 3-D, etc... 141 Example: Disk Obstacle A simple example is when both the vehicle and the obstacle are disks in R 2 . Let the vehicle radius be r and denote the obstacle byO d :=fq d ;r d g, where q d 2R 2 is its center and r d its radius. The obstacle function is d (q;O d ) =kqq d kr d r; so that d (q;O d )> 0 and a homotopy that shrinks the disk can be dened by d (q;O d ) =kqq c k (1)r d r Example: Polygonal Obstacle Polygonal obstacles can be treated in a similar man- ner. LetO p :=fq i g k i=0 denote a polygonal obstacle with k + 1 vertices q 0 ;:::;q k 2 R 2 . Let p (q;O p ) be the distance function between the vehicle at congurationq and polygon O p . If the center of mass of the polygon is q p then a homotopy can be constructed by shrinking the polygon towards its center of mass through p (q;O p ) = p (q;O p ); O p :=fq p + (1)(q i q p )g k i=0 The disk and polygonal homotopies are illustrated in motion planning for a car with simple dynamics. Fig. 5.1 shows dierent stages of the obstacle growth and its aect on the current car trajectory. In the absence of obstacles it is relatively easy to compute an initial path, which can then by quickly deformed to account for the growing obstacles. 142 Figure 5.1: A car traveling in a tunnel. The initial trajectory is quickly computed by shrinking the obstacles to points. The obstacles are then grown and the trajectory re- peatedly modied. 5.3.1.3 Rough Terrain Assume that we have a robotic dog (Fig. 5.2) that must optimally traverse a very rough terrain. One approach is to represent the dog trajectory as a sequence of discrete poses each of which is statically stable in the sense that the center of mass of the dog projects vertically inside the support triangle formed by the set of three feet touching the ground while the fourth one is lifted to move forward. At each pose the dog kinematics must conform to a statically stable touchdown with one leg and statically stable take-o with another leg, in addition to keeping contact with the ground with the remaining legs. For example, Fig. 5.3 shows a few poses along a computed path the traverses a sample terrain. Additionally, the trajectory of the dog must end close to a particular point on the other side of the terrain. The problem is setup as a constrained optimization where 143 Figure 5.2: LittleDog manufactured by Boston Dynamics Inc.. The robot is used in the DARPA \Learning Locomotion" program with the goal to enable an unmanned vehicle to successfully traverse challenging terrains. One such terrain digitized from a real terrain patch is shown on Fig. 5.3. We use it as one of our test environments. the constraints describe the motion and the objective function is the distance to that goal point. The homotopy presented next is designed to increase the chances of successful traversal of rough terrains. It is motivated by the lack of a robust way to nd an initial trajectory that satises the stability constraints and makes progress towards the goal. The idea is to smooth the terrain to a surface that is easy to cross and then to gradually deform that surface into the original terrain with the hope that the trajectory can be corrected to account for the increased diculty of motion. Based on our experiments this can be successfully accomplished, or whenever failure occurs, another easy to compute starting guess can be tried until the task is achieved successfully. 144 Figure 5.3: LittleDog on a digital terrain corresponding to a real laboratory terrain patch. The graphs show a few poses along an optimal path that successfully traverses the terrain. Assume that the terrain height is described by the map z :R 2 !R, i.e. the coordi- nates of each point on the terrain are (x;y;z(x;y)) for (x;y)2R 2 . One way to smooth this surface is to use Gaussian ltering. Dene the following homotopy z (x;y) = 1 m X i=m m X j=m G(;i;j)z(x +i;y +j); (5.3) where the scalar is the terrain map resolution, is the Gaussian lter standard devi- ation, m = round(2=), and G(;v;w) = 8 > > < > > : 1 p 2 e v 2 +w 2 2 2 ; for 6= 0; 1; for = 0; ; = m X i=m m X j=m G(;i;j): 145 Figure 5.4: LittleDog's discrete trajectory on the terrain from Fig. 5.3. The terrain surface is initially smoothed and gradually deformed to its original. As the terrain changes the computed trajectory is adjusted accordingly to satisfy all stability and contact constraints. This type of \embedded" constrained optimization successfully yields an optimal solution, while optimization performed on the original terrain does not converge because of the high irregularity of the terrain. Fig. 5.4 shows several stages of the terrain deformation and the resulting trajectory deformation to account for the changing stability and contact constraints. The nal trajectory satises the contact and stability constraints and could potentially be used as a reference trajectory for successful terrain crossing. Note The map resolution can also be made part of the homotopy since the discrete convolution (5.3) should be performed at low resolution when the Gaussian standard deviation is high. As the standard deviation goes to 0, i.e. when the terrain returns to 146 its original form, the resolution should be increased to its highest value. This can be achieved by replacing in (5.3) with dened by = (1) high + low ; where high is the original resolution of the terrain map, and low is the lowest resolution that would approximate the terrain. 5.3.2 Nonholonomic constraints Assume that we are given a control system on n-dimensional manifold Q with regular distributionD q = spanff i (q);i = 1;:::;cg at q2 Q, where f i : Q! TQ are linearly independent smooth vector elds. Velocity vectors can then be described locally by coordinates v q 2D q R c so that the system has the form _ q = c X i=1 v i f i (q) _ v =u; where u2R c is the control input. Assume that the task is to drive the system from its current state (q(0);v(0)) to a goal state (q(T );v(T )) after some nite timeT , and that the system is controllable given this setup. Let f j :Q!TQ, j =c + 1;:::;n be nc linearly independent vector elds orthogonal toD, i.e. such that f i ;f j = 0, for all i = 1;:::;c and j = c + 1;:::;n. Then T q Q = spanff i (q);i = 1;:::;ng for all q2Q. 147 Let us imagine for a moment that the system was not restricted to move along di- rections inD only, so that it is fully holonomic. Assume that in this case we can easily nd a solution to the motion planning problem and denote this solution trajectory by ( q(t); v(t)), t2 [0;T ]. Then, one way to compute a solution to the real nonholonomic problem is to continuously deform ( q(t); v(t)) by gradually removing the degrees of free- dom corresponding tof j ,j =c + 1;:::;n but keeping the boundary conditions xed. This can be accomplished using the homotopy _ q = (1) c X i=1 v i f i (q) + n X i=1 v i f i (q) _ v =u; Clearly, q 1 (t) = q(t) and q 0 (t) = q(t). The modied system can be used to nd a trajectory satisfying the boundary conditions through a path-lifting method or it could serve as a constraint in an optimal control problem, i.e. minimizing the control eort R T 0 kuk 2 dt. Initial experiments with such homotopy are preformed on a simple nonholonomic unicycle model. Fig. 5.5 shows several stages of the homotopic computation of a control- eort minimizing trajectory. Currently, this type of control system deformation is only an idea and need to be explored in much more depth. 148 Figure 5.5: Homotopy stages of computing an optimal trajectory for a nonholonomic robot. Initially the robot is fully holonomic and a path can be computed very quickly. That path is then gradually deformed to account for the nonholonomic constraint as well as the optimality conditions. 5.4 Conclusion The simple idea behind the methods described in this section was to solve motion plan- ning problems more robustly by relaxing dicult constraints. The developed algorithms apply to iterative numerical schemes arising from root-nding or nonlinear programming motion planning formulation. The approach facilitates the computation of the roots of nonlinear equations or of the minimum of a cost function subject to complex constraints by initially removing, smoothing, or convexifying the constraints and then smoothly trans- forming them back to their original form and adjusting the solution accordingly. Applying the approach to environmental obstacles and to kinematic constraints proved promising in terms of increased eciency (as in the car and helicopter examples) and solution conver- gence when the standard approach fails (as in the rough terrain and dog robot example). 149 Yet, it is a dicult task to provide theoretical performance guarantees for general con- straints. This is an important research direction requiring both local and global analysis of the space of solution trajectories, subject to varying, possibly non-smooth constraints, and optimality conditions. 150 Reference List [1] Eugene Allgower and Kurt Georg. Introduction to Numerical Continuation Methods. SIAM Wiley and Sons, 2003. [2] A. M. Bloch, P. S. Krishnaprasad, J. E. Marsden, and R. Murray. Nonholonomic mechanical systems with symmetry. Arch. Rational Mech. Anal., (136):21{99, 1996. [3] Anthony Bloch. Nonholonomic Mechanics and Control. Springer, 2003. [4] Alexander I. Bobenko and Yuri B. Suris. Discrete lagrangian reduction, discrete euler-poincare equations, and semidirect products. Letters in Mathematical Physics, 49:79, 1999. [5] Nawaf Bou-Rabee and Jerrold E. Marsden. Reduced hamilton-pontryagin variational integrators. preprint, 2007. [6] Francesco Bullo and Andrew Lewis. Geometric Control of Mechanical Systems. Springer, 2004. [7] Elena Celledoni and Brynjulf Owren. Lie group methods for rigid body dynamics and time integration on manifolds. Comput. meth. in Appl. Mech. and Eng., 19(3,4):421{ 438, 2003. [8] H. Cendra, J. E. Marsden, S. Pekarsky, and T. S. Ratiu. Variational principles for lie- poisson and hamilton-poincar equations. Moscow Mathematical Journal, 3:833{867, 2003. [9] H. Cendra, J. E. Marsden, and T. S. Ratiu. Lagrangian reduction by stages. Mem. Amer. Math. Soc., 152(722):108, 2001. [10] H. Cendra, J.E. Marsden, and T.S. Ratiu. Geometric mechanics, lagrangian reduc- tion, and nonholonomic systems. In B. Engquist and W. Schmid, editors, Mathemat- ics Unlimited-2001 and Beyond, pages 221{273. Springer-Verlag, New York, 2001. [11] Howie Choset, Kevin M. Lynch, Seth Hutchinson, George A Kantor, Wolfram Bur- gard, Lydia E. Kavraki, and Sebastian Thrun. Principles of Robot Motion: Theory, Algorithms, and Implementations. MIT Press, June 2005. ISBN 0-262-03327-5. 151 [12] J. Cort es, S. Martinez, J. P. Ostrowski, and K. A. McIsaac. Optimal gaits for dynamic robotic locomotion. The International Journal of Robotics Research, 20(9):707{728, 2001. [13] Jorge Cort es. Geometric, Control and Numerical Aspects of Nonholonomic Cystems. Springer, 2002. [14] M. de Leon, D. Martin de Diego, and A. Santamaria-Merino. Geometric integrators and nonholonomic mechanics. Journal of Mathematical Physics, 45(3):1042{1062, 2004. [15] Jaydev P. Desai and Vijay Kumar. Motion planning for cooperating mobile manip- ulators. Journal of Robotic Systems, 16(10):557{579, 1999. [16] Chris Dever, Bernard Mettler, Eric Feron, and Jovan Popovic. Nonlinear trajectory generation for autonomous vehicles via parameterized maneuver classes. Journal of Guidance, Control, and Dynamics, 29(2):289{302, 2006. [17] Chang D. E., S. Shadden, J. E. Marsden, and R. Olfati-Saber. Collision avoidance for multiple agent systems. In IEEE Conference on Decision and Control, volume 42, pages 539{543, 2003. [18] Kanso E., J.E. Marsden, C.W. Rowley, and J. Melli-Huber. Locomotion of articu- lated bodies in a perfect uid. Journal of Nonlinear Science, (15):255{289, 2005. [19] Yuri N. Fedorov and Dmitry V. Zenkov. Discrete nonholonomic ll systems on lie groups. Nonlinearity, 18:2211{2241, 2005. [20] R.C. Fetecau, J.E. Marsden, M. Ortiz, and M. West. Nonsmooth lagrangian me- chanics and variational collision integrators. SIAM Journal on Applied Dynamical Systems, 2(3):381{416, 2003. [21] E. Frazzoli, M. A. Dahleh, and E. Feron. Real-time motion planning for agile au- tonomous vehicles. AIAA Journal of Guidance, Control, and Dynamics, 25(1):116{ 129, 2002. [22] E. Frazzoli, M. A. Dahleh, and E. Feron. Maneuver-based motion planning for nonlinear systems with symmetries. IEEE Transactions on Robotics, 21(6):1077{ 1091, dec 2005. [23] Roberto Giambo, Fabio Giannoni, and Paolo Piccionez. Optimal control on rieman- nian manifolds by interpolation. Math. Control Signals System, 16:278{296, 2003. [24] E. Hairer, Ch. Lubich, and G. Wanner. Geometric Numerical Integration. Num- ber 31 in Springer Series in Computational Mathematics. Springer-Verlag, 2006. [25] I.I. Hussein and A.M. Bloch. Dynamic interpolation on riemannian manifolds: an application to interferometric imaging. In American Control Conference, number 1, pages 685{ 690, 2004. 152 [26] Toshihiro Iwai and Akitomo Tachibana. The geometry and mechanics of multi- particle systems. Annales de l'institut Henri Poincar (A) Physique thorique, 70(5):525{559, 1999. [27] Sameer M. Jalnapurkar, Melvin Leok, Jerrold E. Marsden, and Matthew West. Dis- crete routh reduction. MATH.GEN., 39:5521, 2006. [28] O. Junge, J.E. Marsden, and S Ober-Bl obaum. Discrete mechanics and optimal control. In Proccedings of the 16th IFAC World Congress, 2005. [29] O. Junge, J.E. Marsden, and S. Ober-Bl obaum. Optimal reconguration of formation ying spacecraft - a decentralized approach. In 45th IEEE Conference on Decision and Control, pages 5210{5215, 2006. [30] Eva Kanso and Jerrold Marsden. Optimal motion of an articulated body in a perfect uid. In IEEE Conference on Decision and Control, pages 2511{2516, 2005. [31] Scott Kelly and Richard Murray. Geometric phases and robotic locomotion. Journal of Robotic Systems, 12(6):417{431, 1995. [32] L. Kharevych, Weiwei, Y. Tong, E. Kanso, J.E. Marsden, P. Schroder, and M. Des- brun. Geometric, variational integrators for computer animation. In Eurograph- ics/ACM SIGGRAPH Symposium on Computer Animation, pages 1{9, 2006. [33] Marin Kobilarov and Gaurav Sukhatme. Reference redacted for author anonymity. 2006. [34] Wang-Sang Koon and Jerrold E. Marsden. Optimal control for holonomic and non- holonomic mechanical systems with symmetry and lagrangian reduction. SIAM Journal on Control and Optimization, 35(3):901{929, 1997. [35] S. M. LaValle. Planning Algorithms. Cambridge University Press, Cambridge, U.K., 2006. [36] T. Lee, N.H. McClamroch, and M. Leok. Optimal control of a rigid body using geometrically exact computations on SE(3). In Proc. IEEE Conf. on Decision and Control, 2006. [37] F. Silva Leite, M. Camarinha, and P. Crouch. Elastic curves as solutions of rieman- nian and sub-riemannian control problems. Math. Control Signals Systems, (13):140{ 155, 2000. [38] A. Santamaria Merino M. de Leon, D. Martin de Diego. Geometric numerical inte- gration of nonholonomic systems and optimal control problems. European Journal of Control, 10:520{526, 2004. [39] J. E. Marsden and J. Ostrowski. Symmetries in motion: Geometric foundations of motion control. Nonlinear Sci. Today, 1998. [40] J. E. Marsden and J. Scheurle. The reduced euler-lagrange equations. Fields Inst. Commun., (1):139{164, 1993. 153 [41] J.E. Marsden and M. West. Discrete mechanics and variational integrators. Acta Numerica, 10:357{514, 2001. [42] Jerrold E. Marsden, Sergey Pekarsky, and Steve Shkoller. Discrete euler-poincare and lie-poisson equations. Nonlinearity, 12:1647{1662, 1999. [43] Jerrold E. Marsden and Tudor S. Ratiu. Introduction to Mechanics and Symmetry. Springer, 1999. [44] R. McLachlan and M. Perlmutter. Integrators for nonholonomic mechanical systems. Journal of NonLinear Science, 16:283{328, aug 2006. [45] James Ostrowski. The Mechanics and Control of Undulatory Robotic Locomotion. PhD thesis, California Institute of Technology, 1996. [46] James Ostrowski. Computing reduced equations for robotic systems with constraints and symmetries. IEEE Transactions on Robotics and Automation, pages 111{123, 1999. [47] James P. Ostrowski, Jaydev P. Desai, and Vijay Kumar. Optimal gait selection for nonholonomic locomotion systems. The International Journal of Robotics Research, 19(3):225{237, 2000. [48] Crouch P. and Leite F. S. The dynamic interpolation problem: on riemannian manifolds, lie groups, and symmetric spaces. Journal of Dynamical and Control Systems, 1(2):177{202, 1995. [49] Sam Prentice and Nicholas Roy. The belief roadmap: Ecient planning in linear pomdps by factoring the covariance. In Proceedings of the 13th International Sym- posium of Robotics Research (ISRR), Hiroshima, Japan, November 2007. [50] Shane Ross. Optimal apping strokes for self-propulsion in a perfect uid. In Amer- ican Control Conference, 2006. [51] Elie A. Shammas, Howie Choset, and Alfred A. Rizzi. Motion planning for dynamic variable inertia mechanical systems with non-holonomic constraints. In International Workshop on the Algorithmic Foundations of Robotics, 2006. [52] T. Yanao, W. S. Koon, J. E. Marsden, and I. G. Kevrekidis. Gyration-radius dy- namics in structural transitions of atomic clusters. J. Chem. Physics., (126):1{17, 2007. [53] H. Yoshimura and J.E. Marsden. Dirac structures in lagrangian mechanics part ii: Variational structures. Journal of Geometry and Physics, 57:209{250, dec 2006. 154
Abstract (if available)
Linked assets
University of Southern California Dissertations and Theses
Conceptually similar
PDF
Learning objective functions for autonomous motion generation
PDF
Optimization-based whole-body control and reactive planning for a torque controlled humanoid robot
PDF
Multi-robot strategies for adaptive sampling with autonomous underwater vehicles
PDF
The representation, learning, and control of dexterous motor skills in humans and humanoid robots
PDF
Motion coordination for large multi-robot teams in obstacle-rich environments
PDF
Iterative path integral stochastic optimal control: theory and applications to motor control
PDF
Data-driven robotic sampling for marine ecosystem monitoring
PDF
Informative path planning for environmental monitoring
PDF
Speeding up trajectory planning for autonomous robots operating in complex environments
PDF
Rethinking perception-action loops via interactive perception and learned representations
PDF
Design and control of a two-mode monopod
PDF
From active to interactive 3D object recognition
PDF
Risk-aware path planning for autonomous underwater vehicles
PDF
Intelligent robotic manipulation of cluttered environments
PDF
Identification, control and visually-guided behavior for a model helicopter
PDF
Information theoretical action selection
PDF
Decentralized real-time trajectory planning for multi-robot navigation in cluttered environments
PDF
Bayesian methods for autonomous learning systems
PDF
Learning affordances through interactive perception and manipulation
PDF
Data-driven autonomous manipulation
Asset Metadata
Creator
Kobilarov, Marin
(author)
Core Title
Discrete geometric motion control of autonomous vehicles
School
Viterbi School of Engineering
Degree
Doctor of Philosophy
Degree Program
Computer Science (Robotics
Publication Date
08/08/2008
Defense Date
04/17/2008
Publisher
University of Southern California
(original),
University of Southern California. Libraries
(digital)
Tag
continuation,discrete mechanics,homotopy,motion planning,OAI-PMH Harvest,optimal control,probabilistic roadmap,robotics
Language
English
Contributor
Electronically uploaded by the author
(provenance)
Advisor
Sukhatme, Gaurav S. (
committee chair
), Desbrun, Mathieu (
committee member
), Marsden, Jerrold (
committee member
), Newton, Paul K. (
committee member
), Schaal, Stefan (
committee member
)
Creator Email
marinkobi@gmail.com,mkobilar@usc.edu
Permanent Link (DOI)
https://doi.org/10.25549/usctheses-m1570
Unique identifier
UC1284478
Identifier
etd-kobilarov-2278 (filename),usctheses-m40 (legacy collection record id),usctheses-c127-109355 (legacy record id),usctheses-m1570 (legacy record id)
Legacy Identifier
etd-kobilarov-2278.pdf
Dmrecord
109355
Document Type
Dissertation
Rights
Kobilarov, Marin
Type
texts
Source
University of Southern California
(contributing entity),
University of Southern California Dissertations and Theses
(collection)
Repository Name
Libraries, University of Southern California
Repository Location
Los Angeles, California
Repository Email
cisadmin@lib.usc.edu
Tags
continuation
discrete mechanics
homotopy
motion planning
optimal control
probabilistic roadmap
robotics