# Constraint Control of Nonholonomic Mechanical Systems

- 225 Downloads

## 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.

## Keywords

Suslov’s problem Nonholonomic mechanics Optimal control## Mathematics Subject Classification

70Q05 70E17 70B10 65L10 65P99 65-04 49K15 37J60 70F25 93B05 93B40 93C10 93C15## Notes

### Acknowledgements

This problem has been suggested to us by Prof. D. V. Zenkov during a visit to the University of Alberta. Subsequent discussions and continued interest by Prof. Zenkov to this project are gratefully acknowledged. We also acknowledge fruitful discussions with Profs. A. A. Bloch, D. D. Holm, M. Leok, F. Gay-Balmaz, T. Hikihara, A. Lewis, and H. Yoshimura. There were many helpful exchanges with Prof. H. Oberle, Prof. L. F. Shampine (bvp4c and bvp5c), Prof. F. Mazzia (TOM and bvptwp), J. Willkomm (ADiMat), M. Weinsten (ADiGator), and Prof. A. Rao (GPOPS-II) concerning ODE BVP solvers, automatic differentiation software, and direct method solvers. Both authors of this project were partially supported by the NSERC Discovery Grant and the University of Alberta Centennial Fund. In addition, Stuart Rogers was supported by the FGSR Graduate Travel Award, the IGR Travel Award, the GSA Academic Travel Award, and the AMS Fall Sectional Grad Student Travel Grant. We also thank the Alberta Innovates Technology Funding (AITF) for providing support to the authors through the Alberta Centre for Earth Observation Sciences (CEOS).

## References

- Aceto, L., Mazzia, F., Trigiante, D.: The performances of the code TOM on the Holt problem. In: Antreich, K., Bulirsch, R., Gilg, A., Rentrop, P. (eds.) Modeling, Simulation, and Optimization of Integrated Circuits, pp. 349–360. Springer, Berlin (2003)Google Scholar
- Agrachev, A., Sachkov, Y.: Control Theory from the Geometric Viewpoint. Springer, Berlin (2004)CrossRefzbMATHGoogle Scholar
- Allgower, E.L., Georg, K.: Introduction to Numerical Continuation Methods, vol. 45. SIAM, Philadelphia (2003)CrossRefzbMATHGoogle Scholar
- Arnold, V.I., Kozlov, V.V., Neishtadt, A.I.: Mathematical Aspects of Classical and Celestial Mechanics, 2nd edn. Spinger, Berlin (1997)zbMATHGoogle Scholar
- Ascher, U.M., Spiteri, R.J.: Collocation software for boundary value differential-algebraic equations. SIAM J. Sci. Comput.
**15**(4), 938–952 (1994)MathSciNetCrossRefzbMATHGoogle Scholar - Ascher, U.M., Christiansen, J., Russell, R.D.: Algorithm 569: COLSYS: collocation software for boundary-value ODEs [D2]. ACM Trans. Math. Softw. (TOMS)
**7**(2), 223–229 (1981)CrossRefGoogle Scholar - Ascher, U.M., Mattheij, R.M.M., Russell, R.D.: Numerical Solution of Boundary Value Problems for Ordinary Differential Equations, vol. 13. SIAM, Philadelphia (1994)zbMATHGoogle Scholar
- Auzinger, W., et al.: A collocation code for singular boundary value problems in ordinary differential equations. Numer. Algorithms
**33**(1–4), 27–39 (2003)MathSciNetCrossRefzbMATHGoogle Scholar - Bader, G., Ascher, U.M.: A new basis implementation for a mixed order boundary value ODE solver. SIAM J. Sci. Stat. Comput.
**8**(4), 483–500 (1987)MathSciNetCrossRefzbMATHGoogle Scholar - Bashir-Ali, Z., Cash, J.R., Silva, H.H.M.: Lobatto deferred correction for stiff two-point boundary value problems. Comput. Math. Appl.
**36**(10), 59–69 (1998)MathSciNetCrossRefzbMATHGoogle Scholar - Becerra, V.M.: Solving complex optimal control problems at no cost with PSOPT. In: 2010 IEEE International Symposium on Computer-Aided Control System Design, pp. 1391–1396. IEEE (2010)Google Scholar
- Betts, J.T.: Survey of numerical methods for trajectory optimization. J. Guid. Control Dyn.
**21**(2), 193–207 (1998)MathSciNetCrossRefzbMATHGoogle Scholar - Betts, J.T.: Practical Methods for Optimal Control and Estimation Using Nonlinear Programming, vol. 19. SIAM, Philadelphia (2010)CrossRefzbMATHGoogle Scholar
- Betts, J.T.: Sparse optimization suite (SOS). In: Applied Mathematical Analysis, LLC. (Based on the Algorithms Published in Betts, JT, Practical Methods for Optimal Control and Estimation Using Nonlinear Programming. SIAM Press, Philadelphia, PA. (2010).) (2013)Google Scholar
- Biegler, L.T.: Nonlinear Programming: Concepts, Algorithms, and Applications to Chemical Processes, vol. 10. SIAM, Philadelphia (2010)CrossRefzbMATHGoogle Scholar
- Birkisson, Á., Driscoll, T.A.: Automatic Linearity Detection (2013a). http://eprints.maths.ox.ac.uk/1672/
- Birkisson, Á.: Numerical solution of nonlinear boundary value problems for ordinary differential equations in the continuous framework. Ph.D. thesis, University of Oxford (2013b)Google Scholar
- Birkisson, Á., Driscoll, T.A.: Automatic Fréchet differentiation for the numerical solution of boundary-value problems. ACM Trans. Math. Softw. (TOMS)
**38**(4), 26 (2012)CrossRefzbMATHGoogle Scholar - Bloch, A.M.: Nonholonomic Mechanics and Control, vol. 24. Springer, New York (2003)Google Scholar
- Bloch, A.M., et al.: Nonholonomic mechanical systems with symmetry. Arch. Ration. Mech. Anal.
**136**, 21–99 (1996)MathSciNetCrossRefzbMATHGoogle Scholar - Bloch, A.A., et al.: A geometric approach to the optimal control of nonholonomic mechanical systems. In: Bettiol, P., Cannarsa, P., Colombo, G., Motta, M., Rampazzo, F. (eds.) Analysis and Geometry in Control Theory and its Applications, pp. 35–64. Springer INdAM Series, Basel (2015a)Google Scholar
- Bloch, A.M., Krupka, D., Zenkov, D.V.: The Helmholtz conditions and the method of controlled Lagrangians. In: Zenkov, D.V. (ed.) The Inverse Problem of the Calculus of Variations: Local and Global Theory, pp. 1–31. Atlantic Press, Minneapolis (2015b)Google Scholar
- Boisvert, J.J., Muir, P.H., Spiteri, R.J.: py_bvp: a universal Python interface for BVP codes. In: Proceedings of the 2010 Spring Simulation Multiconference, p. 95. Society for Computer Simulation International (2010)Google Scholar
- Boisvert, J.J., Muir, P.H., Spiteri, R.J.: A Runge–Kutta BVODE solver with global error and defect control. ACM Trans. Math. Softw. (TOMS)
**39**(2), 11 (2013)MathSciNetCrossRefzbMATHGoogle Scholar - Bonnans, F., et al.: BOCOP: User Guide (2014). http://bocop.org/
- Bonnans, F., et al.: BocopHJB 1.0. 1–User Guide, PhD thesis. INRIA (2015)Google Scholar
- Bonnard, B., Caillau, J.-B., Trélat, E.: Computation of conjugate times in smooth optimal control: the COTCOT algorithm. In: Proceedings of the 44th IEEE Conference on Decision and Control and European Control Conference 2005 (CDC-ECC’05), Séville, Spain. IEEE (2005)Google Scholar
- Bonnard, B., Caillau, J.-B., Trélat, E.: Second order optimality conditions in the smooth case and applications in optimal control. ESAIM Control Optim. Calc. Var.
**13**(2), 207–236 (2007)MathSciNetCrossRefzbMATHGoogle Scholar - Borisov, A.V., Kilin, A.A., Mamaev, I.S.: Hamiltonicity and integrability of the Suslov problem. Regul. Chaotic Dyn.
**16**(1–2), 104–116 (2011)MathSciNetCrossRefzbMATHGoogle Scholar - Brauer, G.L., Cornick, D.E., Stevenson, R.: Capabilities and Applications of the Program to Optimize Simulated Trajectories (POST). Program Summary Document (1977)Google Scholar
- Brockett, R.W.: System theory on group manifolds and coset spaces. SIAM J. Control
**10**(2), 265–284 (1972)MathSciNetCrossRefzbMATHGoogle Scholar - Brugnano, L., Trigiante, D.: A new mesh selection strategy for ODEs. Appl. Numer. Math.
**24**(1), 1–21 (1997)MathSciNetCrossRefzbMATHGoogle Scholar - Bryson, A.E.: Applied Optimal Control: Optimization, Estimation and Control. CRC Press, Boca Raton (1975)Google Scholar
- Bryson, A.E.: Dynamic Optimization, vol. 1. Prentice Hall, Englewood Cliffs (1999)Google Scholar
- Bullo, F., Lewis, A.D.: Geometric Control of Mechanical Systems: Modeling, Analysis, and Design for Simple Mechanical Control Systems. Texts in Applied Mathematics. Springer, New York (2005)CrossRefzbMATHGoogle Scholar
- Byrd, R.H., Nocedal, J., Waltz, R.A.: KNITRO: an integrated package for nonlinear optimization. In: Di Pillo, G., Roma, M. (eds.) Large-Scale Nonlinear Optimization, pp. 35–59. Springer, Berlin (2006)Google Scholar
- Caillau, J.-B., Cots, O., Gergaud, J.: Differential continuation for regular optimal control problems. Optim. Methods Softw.
**27**(2), 177–196 (2012)MathSciNetCrossRefzbMATHGoogle Scholar - Cash, J.R., Wright, M.H.: A deferred correction method for nonlinear two-point boundary value problems: implementation and numerical evaluation. SIAM J. Sci. Stat. Comput.
**12**(4), 971–989 (1991)MathSciNetCrossRefzbMATHGoogle Scholar - Cash, J.R., Mazzia, F.: A new mesh selection algorithm, based on conditioning, for two-point boundary value codes. J. Comput. Appl. Math.
**184**(2), 362–381 (2005)MathSciNetCrossRefzbMATHGoogle Scholar - Cash, J.R., Mazzia, F.: Hybrid mesh selection algorithms based on conditioning for two-point boundary value problems. JNAIAM J. Numer. Anal. Ind. Appl. Math.
**1**(1), 81–90 (2006)MathSciNetzbMATHGoogle Scholar - Cash, J.R., Moore, G., Wright, R.W.: An automatic continuation strategy for the solution of singularly perturbed nonlinear boundary value problems. ACM Trans. Math. Softw. (TOMS)
**27**(2), 245–266 (2001)CrossRefzbMATHGoogle Scholar - Cash, J.R., et al.: The role of conditioning in mesh selection algorithms for first order systems of linear two point boundary value problems. J. Comput. Appl. Math.
**185**(2), 212–224 (2006)MathSciNetCrossRefzbMATHGoogle Scholar - Cash, J.R., et al.: Algorithm 927: the MATLAB code bvptwp. m for the numerical solution of two point boundary value problems. ACM Trans. Math. Softw. (TOMS)
**39**(2), 15 (2013)MathSciNetCrossRefzbMATHGoogle Scholar - Chen, Y.Q., Schwartz, A.L.: RIOTS95: a MATLAB toolbox for solving general optimal control problems and its applications to chemical processes (2002). http://www.schwartz-home.com/riots/
- Chow, Y.T., et al.: Algorithm for overcoming the curse of dimensionality for certain non-convex Hamilton–Jacobi equations, projections and differential games. Ann. Math. Sci. Appl. (2016, to appear)Google Scholar
- Cizniar, M., et al.: Dynopt–dynamic optimisation code for MATLAB. In: Technical Computing Prague 2005 (2005)Google Scholar
- Community Portal for Automatic Differentiation (2016). http://www.autodiff.org/. Visited 10/08/2016
- Cortés, J., Martínez, E.: Mechanical control systems on Lie algebroids. IMA J. Math. Control Inf.
**21**, 457–492 (2004)MathSciNetCrossRefzbMATHGoogle Scholar - Dahlquist, G., Björck, A.: Numerical Methods. Series in Automatic Computation. Prentice-Hall, Englewood Cliffs (1974)zbMATHGoogle Scholar
- Dankowicz, H., Schilder, F.: Recipes for Continuation. SIAM, Philadelphia (2013)CrossRefzbMATHGoogle Scholar
- Darbon, J., Osher, S.: Algorithms for Overcoming the Curse of Dimensionality for Certain Hamilton–Jacobi Equations Arising in Control Theory and Elsewhere. arXiv preprint arXiv:1605.01799 (2016)
- Doedel, E.J., et al.: AUTO-07P: Continuation and Bifurcation Software for Ordinary Differential Equations (2007)Google Scholar
- Driscoll, T.A., Hale, N., Trefethen, L.N.: Chebfun Guide (2014). Pafnuty Publications. http://www.chebfun.org/docs/guide/
- Enright, W.H., Muir, P.H.: Runge–Kutta software with defect control for boundary value ODEs. SIAM J. Sci. Comput.
**17**(2), 479–497 (1996)MathSciNetCrossRefzbMATHGoogle Scholar - Evans, L.C.: Partial Differential Equations. American Mathematical Society, Providence. ISBN: 9780821849743, 0821849743 (2010)Google Scholar
- Falugi, P., Kerrigan, E., Van Wyk, E.: Imperial College london Optimal Control Software User Guide (ICLOCS). Department of Electrical and Electronic Engineering, Imperial College London, London (2010)Google Scholar
- Gay-Balmaz, F., Putkaradze, V.: On noisy extensions of nonholonomic constraints. J. Nonlinear Sci.
**26**(6), 1571–1613 (2016). doi: 10.1007/s00332-016-9313-x - Gerdts, M.: Optimal Control of ODEs and DAEs. Walter de Gruyter, Berlin (2012)CrossRefzbMATHGoogle Scholar
- Gill, P.E., Wong, E.: Users Guide for SNCTRL. (2015). https://ccom.ucsd.edu/research/abstract.php?id=102
- Gill, P.E., Murray, W., Saunders, M.A.: SNOPT: an SQP algorithm for large-scale constrained optimization. SIAM Rev.
**47**(1), 99–131 (2005)MathSciNetCrossRefzbMATHGoogle Scholar - Goh, C.J., Teo, K.L.: MISER: a FORTRAN program for solving optimal control problems. Adv. Eng. Softw. (1978)
**10**(2), 90–99 (1988)CrossRefGoogle Scholar - Hale, N., Moore, D.R.: A sixth-order extension to the MATLAB package bvp4c of J. Kierzenka and L. Shampine (2008)Google Scholar
- Holm, D.D.: Geometric mechanics: rotating, translating, and rolling. In: Geometric Mechanics. Imperial College Press, London. ISBN: 9781848167773 (2011)Google Scholar
- Holmström, K., Göran, A., Edvall, M.M.: Users Guide for Tomlab 7 (2010). Tomlab Optimization, Vallentuna and Västerås. http://tomopt.com/tomlab/download/manuals.php
- Houska, B., Ferreau, H.J., Diehl, M.: ACADO toolkitAn open-source framework for automatic control and dynamic optimization. Optim. Control Appl. Methods
**32**(3), 298–312 (2011)CrossRefzbMATHGoogle Scholar - Hull, D.G.: Optimal Control Theory for Applications. Springer, Berlin (2013)Google Scholar
- Isidori, A.: Nonlinear Control Systems. Springer, Berlin (2013)zbMATHGoogle Scholar
- Jansch, C., Well, K.H., Schnepper, K.: GESOP-Eine Software Umgebung Zur Simulation Und Optimierung. In: Proceedings des SFB (1994)Google Scholar
- Jean, F.: Control of Nonholonomic Systems: From Sub-Riemannian Geometry to Motion Planning. Springer, Cham (2014)zbMATHGoogle Scholar
- Kelly, M.P.: OptimTraj User’s Guide, Version 1.5. (2016). https://github.com/MatthewPeterKelly/OptimTraj/tree/master/docs/UsersGuide
- Kierzenka, J., Shampine, L.F.: A BVP solver that controls residual and error. JNAIAM J. Numer. Anal. Ind. Appl. Math.
**3**(1–2), 27–41 (2008)MathSciNetzbMATHGoogle Scholar - Kitzhofer, G., et al.: The new Matlab code bvpsuite for the solution of singular implicit BVPs. J. Numer. Anal. Ind. Appl. Math
**5**, 113–134 (2010)MathSciNetzbMATHGoogle Scholar - Koon, W.-S., Marsden, J.E.: Optimal control for holonomic and nonholonomic mechanical system with symmetry and Lagrangian reduction. SIAM J. Control Optim.
**35**, 901–929 (1997)MathSciNetCrossRefzbMATHGoogle Scholar - Kozlov, V.V.: On the integration theory of equations of nonholonomic mechanics. Regul. Chaotic Dyn.
**7**(2), 161–176 (2002)MathSciNetCrossRefzbMATHGoogle Scholar - Kunkel, P.: Differential-Algebraic Equations: Analysis and Numerical Solution. European Mathematical Society, Germany (2006)CrossRefzbMATHGoogle Scholar
- Lee, J.M.: Smooth Manifolds. Springer, Berlin (2003)CrossRefGoogle Scholar
- Lewis, A.D.: Simple mechanical control systems with constraints. IEEE Trans. Automat. Control
**45**, 14201436 (2000)MathSciNetGoogle Scholar - Lewis, A.D., Murray, R.M.: Variational principles for constrained systems: theory and experiment. Int. J. Non Linear Mech.
**30**(6), 793–815 (1995)MathSciNetCrossRefzbMATHGoogle Scholar - Lewis, A.D., Murray, R.M.: Controllability of simple mechanical control systems. SIAM J. Control Optim.
**35**, 766–790 (1997)MathSciNetCrossRefzbMATHGoogle Scholar - Li, Z., Canny, J.F. (eds.): Nonholonomic Motion Planning. Springer, New York (1992)zbMATHGoogle Scholar
- Marsden, J.E., Ratiu, T.S.: Introduction to Mechanics and Symmetry: A Basic Exposition of Classical Mechanical Systems, vol. 17. Springer, Berlin (2013)zbMATHGoogle Scholar
- Mazzia, F., Sgura, I.: Numerical approximation of nonlinear BVPs by means of BVMs. Appl. Numer. Math.
**42**(1), 337–352 (2002)MathSciNetCrossRefzbMATHGoogle Scholar - Mazzia, F., Trigiante, D.: A hybrid mesh selection strategy based on conditioning for boundary value ODE problems. Numer. Algorithms
**36**(2), 169–187 (2004)MathSciNetCrossRefzbMATHGoogle Scholar - Mazzia, F., Cash, J.R., Soetaert, K.: Solving boundary value problems in the open source software R: package bvpSolve. Opusc. Math.
**34**(2), 387–403 (2014)MathSciNetCrossRefzbMATHGoogle Scholar - Nijmeijer, H., Van der Schaft, A.: Nonlinear Dynamical Control Systems. Springer, Berlin (2013)zbMATHGoogle Scholar
- Nikolayzik, T., Büskens, C., Gerdts, M.: Nonlinear large-scale optimization with WORHP. In: Proceedings of the 13th AIAA/ISSMO Multidisciplinary Analysis Optimization Conference, vol. 13, no. 15.09 (2010)Google Scholar
- Oberle, H.J., Grimm, W.: BNDSCO: A Program for the Numerical Solution of Optimal Control Problems. Inst. für Angewandte Math. der Univ, Hamburg (2001)Google Scholar
- Osborne, J., Zenkov, D.V.: Steering the Chaplygin sleigh by a moving mass. In: Proceedings of CDC 2005, vol. 44, pp. 1114–1118Google Scholar
- Patterson, M.A., Rao, A.V.: GPOPS-II: a MATLAB software for solving multiple-phase optimal control problems using hp-adaptive Gaussian quadrature collocation methods and sparse nonlinear programming. ACM Trans. Math. Softw. (TOMS)
**41**(1), 1 (2014)MathSciNetCrossRefzbMATHGoogle Scholar - Poincaré, H.: Sur une forme nouvelle des équations de la mécanique. CR Acad. Sci
**132**, 369–371 (1901)zbMATHGoogle Scholar - Rao, A.V.: A survey of numerical methods for optimal control. Adv. Astronaut. Sci.
**135**(1), 497–528 (2009)Google Scholar - Rao, A.V., et al.: Algorithm 902: Gpops, a matlab software for solving multiple-phase optimal control problems using the gauss pseudospectral method. ACM Trans. Math. Softw. (TOMS)
**37**(2), 22 (2010)CrossRefzbMATHGoogle Scholar - Rieck, M., et al.: FALCON.m User Guide. Institute of Flight System Dynamics, Technische Universität München, Munich (2016)Google Scholar
- Ross, I.M.: Users Manual for DIDO: A MATLAB Application Package for Solving Optimal Control Problems. Elissar Global (2004). http://www.elissarglobal.com/academic/products/
- Rutquist, P.E., Edvall, M.M.: Propt-matlab Optimal Control Software, vol. 260. Tomlab Optimization Inc, Vallentuna and Västerås (2010)Google Scholar
- Shampine, L.F., Kierzenka, J., Reichelt, M.W.: Solving boundary value problems for ordinary differential equations in MATLAB with bvp4c. In: Tutorial notes, pp. 437–448 (2000)Google Scholar
- Shampine, L.F., Muir, P.H., Xu, H.: A user-friendly Fortran BVP solver1. JNAIAM
**1**(2), 201–217 (2006)MathSciNetzbMATHGoogle Scholar - Suslov, G.K.: Theoretical Mechanics, vol. 3. Gostekhizdat, Moscow (1946)Google Scholar
- The Numerical Algorithms Group (NAG). The NAG Library. www.nag.com
- Vlases, W.G., et al.: Optimal trajectories by implicit simulation. In: Boeing Aerospace and Electronics, Technical Report WRDC-TR-90-3056, Wright-Patterson Air Force Base (1990)Google Scholar
- von Stryk, O.: Users Guide for DIRCOL. Technische Universität Darmstadt, Darmstadt (2000)Google Scholar
- Wächter, A., Biegler, L.T.: On the implementation of an interior-point filter line-search algorithm for large-scale nonlinear programming. Math. Program.
**106**(1), 25–57 (2006)MathSciNetCrossRefzbMATHGoogle Scholar - Weinstein, M.J., Patterson, M.A., Rao, A.V.: Utilizing the algorithmic differentiation package ADiGator for solving optimal control problems using direct collocation. In: AIAA Guidance, Navigation, and Control Conference, p. 1085 (2015)Google Scholar
- Weinstein, M.J., Rao, A.V.: Algorithm: ADiGator, a toolbox for the algorithmic differentiation of mathematical functions in MATLAB using source transformation via operator overloading. ACM Trans. Math. Softw. (2016, in revision). http://www.anilvrao.com/Publications/SubmittedJournalPublications/adigator-CALGO.pdf
- Zenkov, D.V., et al.: Matching and stabilization of low dimensional nonholonomic systems. In: Proc CDC 2000, vol 39. pp. 1289–1295 (2000)Google Scholar
- Zenkov, D.V., Bloch, A.M., Marsden, J.E.: Flat nonholonomic matching. In: Proceedings of ACC 2002, pp. 2812–2817 (2002)Google Scholar