Constraint Control of Nonholonomic Mechanical Systems

Abstract

We derive an optimal control formulation for a nonholonomic mechanical system using the nonholonomic constraint itself as the control. We focus on Suslov’s problem, which is defined as the motion of a rigid body with a vanishing projection of the body frame angular velocity on a given direction $\varvec{\xi }$. We derive the optimal control formulation, first for an arbitrary group, and then in the classical realization of Suslov’s problem for the rotation group $\textit{SO}(3)$. We show that it is possible to control the system using the constraint $\varvec{\xi }(t)$ and demonstrate numerical examples in which the system tracks quite complex trajectories such as a spiral.

A Survey of Numerical Methods for Solving Optimal Control Problems: Dynamic Programming, the Direct Method, and the Indirect Method

There are three approaches to solving an optimal control problem: (1) dynamic programming, (2) the direct method, and (3) the indirect method. Bryson (1975), Bryson (1999) present an introduction to dynamic programming. Betts (1998), Rao (2009) are thorough survey articles on the direct and indirect methods. Reference Gerdts (2012) is a recent treatise providing detailed descriptions of both the direct and indirect methods, Biegler (2010) is a comprehensive reference on the direct method, while Betts (2010) provides a comprehensive, modern treatment of the local collocation technique of the direct method.

In dynamic programming, a PDE, called the Hamilton–Jacobi–Bellman equation (Evans 2010), is formulated and solved. However, due to the curse of dimensionality, solution of this PDE is only practical for very simple problems. Therefore, very few numerical solvers implement dynamic programming to solve optimal control problems. For example, BOCOPHJB (Bonnans et al. 2015) is free C++ software implementing the dynamic programming approach. Darbon and Osher (2016), Chow (2016) constitute recent research that seeks to overcome the curse of dimensionality in certain special cases.

Note that because the control function u is an unknown function of time, an optimal control problem is infinite-dimensional. In the direct method, the infinite-dimensional optimal control problem is approximated by a finite-dimensional nonlinear programming (NLP) problem by parameterizing the control function u as a finite linear combination of basis functions. In the sequential approach of the direct method, the state is reconstructed from a guess of the unknown coefficients for the control basis functions, the unknown parameters, the unknown initial states, and the unknown terminal time by multiple shooting. In the simultaneous approach of the direct method, the state is also parameterized as a finite linear combination of basis functions. In the direct method, the ODE, initial boundary conditions, terminal boundary conditions, and path constraints are represented as a system of algebraic inequalities, and the objective function is minimized subject to satisfying the system of algebraic inequalities, with the unknowns being the coefficients for the control and/or state basis functions, parameters, initial states, and the terminal time. In the Lagrange and Bolza formulations, the objective function is approximated via numerical quadrature. There are many NLP problem solvers available, such as IPOPT (Wächter and Biegler 2006), WORHP (Nikolayzik et al. 2010), SNOPT (Gill et al. 2005), KNITRO (Byrd et al. 2006), and $\texttt {MATLAB}$’s $\texttt {fmincon}$;¹ of these NLP problem solvers, only IPOPT and WORHP are free. Most direct method solvers utilize one of these NLP problem solvers.

The packages RIOTS (Chen and Schwartz 2002), DYNOPT (Cizniar et al. 2005), ICLOCS (Falugi et al. 2010), GPOPS (Rao et al. 2010), FALCON.m (Rieck et al. 2016), and OptimTraj (Kelly 2016) are free MATLAB implementations of the direct method, while DIDO (Ross 2004), PROPT (Rutquist and Edvall 2010), and GPOPS-II (Patterson and Rao 2014) are commercial MATLAB implementations of the direct method. BOCOP (Bonnans et al. 2014), ACADO (Houska et al. 2011), and PSOPT (Becerra 2010) are free C++ implementations of the direct method. MISER (Goh and Teo 1988), DIRCOL (von Stryk 2000), SNCTRL (Gill and Wong 2015), OTIS (Vlases et al. 1990), and POST (Brauer et al. 1977) are free Fortran implementations of the direct method, while GESOP (Jansch et al. 1994) and SOS (Betts 2013) are commercial Fortran implementations of the direct method.

In the indirect method, Pontryagin’s minimum principle uses the calculus of variations to formulate necessary conditions for a minimum solution to the optimal control problem. These necessary conditions take the form of a differential algebraic equation (DAE) with boundary conditions; such a problem is called a DAE boundary value problem (BVP). In some cases, through algebraic manipulation, it is possible to convert the DAE to an ordinary differential equation (ODE), thereby producing an ODE BVP.

A DAE BVP can be solved numerically by multiple shooting, collocation, or quasilinearization (Kunkel 2006). bvpsuite (Kitzhofer et al. 2010) is a free MATLAB collocation DAE BVP solver. COLDAE (Ascher and Spiteri 1994) is a free Fortran quasilinearization DAE BVP solver, which solves each linearized problem via collocation. The commercial Fortran code SOS, mentioned previously, also has the capability to solve DAE BVPs arising from optimal control problems via multiple shooting or collocation.

An ODE BVP can be solved numerically by multiple shooting, Runge–Kutta methods, collocation (which is a special subset of Runge–Kutta methods), finite differences, or quasilinearization (Ascher et al. 1994). bvp4c (Shampine et al. 2000), bvp5c (Kierzenka and Shampine 2008), bvp6c (Hale and Moore 2008), and sbvp (Auzinger et al. 2003) are MATLAB collocation ODE BVP solvers; bvp4c and bvp5c come standard with MATLAB, while bvp6c and sbvp are free. bvptwp (Cash et al. 2013) is a free MATLAB ODE BVP solver package implementing 6 algorithms: twpbvp_m, twpbvpc_m, twpbvp_l, twpbvpc_l, acdc, and acdcc; acdc and acdcc perform automatic continuation. twpbvp_m and twpbvpc_m rely on Runge–Kutta methods, while the other 4 algorithms rely on collocation. TOM (Brugnano and Trigiante 1997; Mazzia and Sgura 2002; Aceto et al. 2003; Mazzia and Trigiante 2004; Cash et al. 2006) is a free MATLAB quasilinearization ODE BVP solver, which uses finite differences to solve each linearized problem. solvebvp (Birkisson 2013b; Birkisson and Driscoll 2012, 2013a) is a MATLAB quasilinearization ODE BVP solver available in the free MATLAB toolbox Chebfun (Driscoll et al. 2014); solvebvp uses spectral collocation to solve each linearized problem. COTCOT (Bonnard et al. 2005), HamPath (Caillau et al. 2012), and BNDSCO (Oberle and Grimm 2001) are free Fortran indirect method optimal control problem solvers that use multiple shooting to solve the ODE BVPs.

MIRKDC (Enright and Muir 1996), BVP_SOLVER (Shampine et al. 2006), and BVP_SOLVER-2 (Boisvert et al. 2013) and TWPBVP (Cash and Wright 1991) and TWPBVPC (Cash and Mazzia 2005) are free Fortran Runge–Kutta method ODE BVP solvers. TWPBVPL (Bashir-Ali et al. 1998), TWPBVPLC (Cash and Mazzia 2006), ACDC (Cash et al. 2001), and ACDCC (Cash et al. 2013) are free Fortran collocation ODE BVP solvers. bvptwp, mentioned previously, is a MATLAB reimplementation of the Fortran solvers TWPBVP, TWPBVPC, TWPBVPL, TWPBVPLC, ACDC, and ACDCC. COLSYS (Ascher et al. 1981), COLNEW (Bader and Ascher 1987), and COLMOD (Cash et al. 2001) are free Fortran collocation quasilinearization ODE BVP solvers; COLMOD is an automatic continuation version of COLNEW. bvpSolve (Mazzia et al. 2014) is an R library that wraps the Fortran solvers TWPBVP, TWPBVPC, TWPBVPL, TWPBVPLC, ACDC, and ACDCC and COLSYS, COLNEW, COLMOD, and COLDAE. py_bvp (Boisvert et al. 2010) is a Python library that wraps the Fortran solvers TWPBVPC, COLNEW, and BVP_SOLVER. The NAG Library (www.nag.com) is a commercial Fortran library consisting of several multiple shooting, collocation, and finite difference ODE BVP solvers. The Fortran solvers in the NAG Library are accessible from other languages (like C, Python, MATLAB, and .NET) via wrappers.

The NLP solver utilized by a direct method and the ODE/DAE BVP solver utilized by an indirect method must compute Jacobians and/or Hessians (i.e., derivatives) of the functions involved in the optimal control problem. These derivatives may be approximated by finite differences, but for increased accuracy and in many cases increased efficiency, exact (to machine precision) derivatives are desirable. These exact derivatives may be computed through symbolic or automatic differentiation. Usually, a symbolic derivative evaluates much more rapidly than an automatic derivative; however, due to expression explosion, symbolic derivatives cannot always be obtained for very complicated functions. The Symbolic Math Toolbox and TomSym (Holmström et al. 2010) are commercial MATLAB toolboxes that compute symbolic derivatives. While the Symbolic Math Toolbox only computes un-vectorized symbolic derivatives, TomSym computes both un-vectorized and vectorized symbolic derivatives. ADiGator (Weinstein and Rao 2016; Weinstein et al. 2015) is a free MATLAB toolbox capable of computing both un-vectorized and vectorized automatic derivatives. Usually in MATLAB, a vectorized automatic derivative evaluates much more rapidly than an un-vectorized symbolic derivative (wrapped within a for loop). Only the MATLAB Symbolic Math Toolbox and ADiGator were utilized in this research. For other automatic differentiation packages available in many programming languages, see Community Portal for Automatic Differentiation (2016).

A dynamic programming solution satisfies necessary and sufficient conditions for a global minimum solution of an optimal control problem. A direct method solution satisfies necessary and sufficient conditions for a local minimum solution of a finite-dimensional approximation of an optimal control problem, while an indirect method solution only satisfies necessary conditions for a local minimum solution of an optimal control problem. Thus, the dynamic programming approach is the holy grail for solving an optimal control problem; however, as mentioned previously, dynamic programming is impractical due to the curse of dimensionality. Therefore, in practice only direct and indirect methods are used to solve optimal control problems.

Since the direct method solves a finite-dimensional approximation of the original optimal control problem, the direct method is not as accurate as the indirect method. Moreover, the indirect method converges much more rapidly than the direct method. However, in addition to solving for the states and controls, the indirect method must also solve for the costates. Since the costates are unphysical, they are very difficult to guess initially. Therefore, though the direct method may be slower than the indirect method and may not be quite as accurate as the indirect method, the direct method is much more robust to poor initial guesses of the states and controls. Therefore, the preferred method of solution for many practical applications tends to be the direct method.

In some cases, it is possible to surmount the problem of providing a good initial guess required to obtain convergence via the indirect method. In such cases, the indirect method will converge substantially faster than the direct method. If a solution of a simpler optimal control problem is known and if the simpler and original optimal control problems are related by a continuous parameter, it may be possible to perform numerical continuation in the parameter from the solution of the simpler optimal control problem to a solution of the original optimal control problem. In the literature, numerical continuation is also sometimes called the differential path following method or the homotopy method. Allgower and Georg (2003) is a comprehensive reference on numerical continuation methods. bvpsuite implements a continuation algorithm to solve DAE BVPs, where the continuation parameter may have turning points (i.e., the continuation parameter need not monotonically increase or decrease). COCO (Dankowicz and Schilder 2013), a free collection of MATLAB toolboxes, and AUTO (Doedel et al. 2007), free Fortran software, implement sophisticated algorithms for the numerical continuation (permitting turning points) of ODE BVPs. followpath, available in the MATLAB toolbox Chebfun, is able to utilize Chebfun’s quasilinearization ODE BVP solver bvpsolve to solve ODE BVPs via continuation, where the continuation parameter may have turning points. acdc / ACDC, acdcc / ACDCC, and COLMOD implement continuation algorithms to solve ODE BVPs, but the continuation parameter is assumed to monotonically increase or decrease. HamPath, mentioned previously, is a free Fortran indirect method optimal control problem solver which uses continuation (permitting turning points) in concert with multiple shooting. Of these numerical continuation tools, only acdc and acdcc were used in this research.

In order to converge to the solution of the true optimal control problem rather than a finite-dimensional approximation, a direct method may use h, p, or hp methods. In the h method, the degree of the approximating polynomial on each mesh interval is held fixed, while the mesh is adaptively refined until the solution meets given error tolerances. In the p method, the mesh is held fixed, while the degree of the approximating polynomial on each mesh interval is adaptively increased until the solution meets given error tolerances. In the hp method, which is implemented by GPOPS-II, the mesh and the degree of the approximating polynomial on each mesh interval are adaptively refined until the solution meets given error tolerances.

We have used the indirect method to numerically solve Suslov’s optimal control problem, as it is vastly superior in speed compared to other methods and is capable of dealing with solutions having sharp gradients, if an appropriate BVP solver is utilized. This was made possible by constructing an analytical state and control solution to a singular optimal control problem and then by using continuation in the cost function coefficients to solve the actual optimal control problem. The singular optimal control problem can be solved analytically since the initial conditions $\varvec{\xi }(a)$ are not prescribed, because the constraint $\left\langle \varvec{\Omega }(a),\varvec{\xi }(a)\right\rangle =0$ is not explicitly enforced, and because it is easy to solve Suslov’s pure equations of motion for $\varvec{\xi }$ in terms of $\varvec{\Omega }$. Since the necessary conditions obtained by applying Pontryagin’s minimum principle to Suslov’s optimal control problem can be formulated as an ODE BVP, ODE rather than DAE BVP solvers were used. The MATLAB solvers bvp4c, bvp5c, bvp6c, sbvp, bvptwp, and TOM were used to solve the ODE BVP. sbvp and bvptwp, which have up to 8th-order accuracy, were found to be the most robust in solving the ODE BVP for Suslov’s optimal control problem; the other MATLAB solvers were found to be very inefficient, requiring many thousands of mesh points due to their lower accuracy. The numerical results presented in Sect. 5.3 were obtained via bvptwp’s automatic continuation solver acdc and sbvp.