Multibody System Dynamics

, Volume 17, Issue 2–3, pp 119–139 | Cite as

real-time motion planning for multibody systems

Real life application examples
  • Enrico Bertolazzi
  • Francesco Biral
  • Mauro Da Lio


The solution of constrained motion planning is an important task in a wide number of application fields. The real-time solution of such a problem, formulated in the framework of optimal control theory, is a challenging issue. We prove that a real-time solution of the constrained motion planning problem for multibody systems is possible for practical real-life applications on standard personal computers.

The proposed method is based on an indirect approach that eliminates the inequalities via penalty formulation and solves the boundary value problem by a combination of finite differences and Newton–Broyden algorithm. Two application examples are presented to validate the method and for performance comparisons. Numerical results show that the approach is real-time capable if the correct penalty formulation and settings are chosen.


Optimal control Penalty formulation Newton method Broyden update Real-time Multibody 


Unable to display preview. Download preview PDF.

Unable to display preview. Download preview PDF.


  1. 1.
    Büskens, C., Knauer, M.: Hihger order real-time approximations in optimal control of multibody-systems for industrial robots. J. Multibody Syst. Dyn. 15, 85–106 (2006)CrossRefMATHGoogle Scholar
  2. 2.
    Diehl, M., Findeisen, R., Allgöwer, F.: A stabilizing real-time implementation of nonlinear model predictive control. In: Biegler, L., Ghattas, O., Heinkenschloss, M., Keyes, D., van Bloem Wanders, B. (eds.) Proceedings of Sandia Real-Time PDE Optimization Workshop, Springer-Verlag (in press)Google Scholar
  3. 3.
    Frazzoli, E., Dahleh, M.A., Feron, E.: Real-time motion planning for agile autonomous vehicles. In: AIAA Guidance, Navigation, and Control Conference and Exhibit. Number 4056 in AIAA (2000)Google Scholar
  4. 4.
    Milam, M.B.: Real-Time Optimal Trajectory Generation for Constrained Dynamical Systems. Dissertation, California Institute of Technology (2003)Google Scholar
  5. 5.
    Prasanth Kumar, R., Dasgupta, A., Kumar, C.S.: Real-time optimal motion planning for autonomous underwater vehicles. Ocean Eng. 32(11–12), 1431–1447 (2005)CrossRefGoogle Scholar
  6. 6.
    Bertolazzi, E., Biral, F., Da Lio, M.: Future advanced driver assistance systems based on optimal control: the influence of “risk functions” on overall system behavior and on prediction of dangerous situation. In: Proceedings of IEEE Intelligent Vehicles Symposium, pp. 386–390 (2004)Google Scholar
  7. 7.
    Biral, F., Da Lio, M.: Modelling drivers with the optimal manoeuvre method. In: ATA 2001, 7th International Conference and Exhibition (2001)Google Scholar
  8. 8.
    Biral, F., Da Lio, M., Bertolazzi, E.: Combining safety margins and user preferences into a driving criterion for optimal control-based computation of reference maneuvers for an ADAS of the next generation. In: Proceedings of IEEE Intelligent Vehicles Symposium, pp. 36–41 (2005)Google Scholar
  9. 9.
    Bottasso, C., Chang, C.S., Croce, A., Leonello, D., Riviello, L.: Adaptive planning and tracking of trajectories for the simulation of maneuvers with multibody models. Comput. Methods Appl. Mech. Eng. 195(50–51), 7052–7072 (2006)CrossRefMathSciNetMATHGoogle Scholar
  10. 10.
    Petti, S., Fraichard, T.: Safe motion planning in dynamic environments. In: Proceedings of the IEEE-RSJ International Conference on Intelligent Robots and Systems, Edmonton, AB (2005)Google Scholar
  11. 11.
    Bruce, J., Veloso, M.: Real-time randomized path planning for robot navigation. In: Proceedings of the IEEE-RSJ Intelligent Robots and Systems, pp. 2383–2388 (2002)Google Scholar
  12. 12.
    LaValle, S.M., Kuffner, J.J.: Randomized kinodynamic planning. Int. J. Robot. Res. 20(5), 378–400 (2001)CrossRefGoogle Scholar
  13. 13.
    Cerven, W.T., Bullo, F., Coverstone, V.L.: Vehicle motion planning with time-varying constraints. J. Guid. Control Dyn. 27(3), 506–509 (2004)CrossRefGoogle Scholar
  14. 14.
    Cuthrell, J., Biegler, L.: Simultaneous optimization and solution methods for batch reactor control profiles. Comput. Chem. Eng. 13(1–2), 49–62 (1989)CrossRefGoogle Scholar
  15. 15.
    von Stryk, O., Bulirsch, R.: Direct and indirect methods for trajectory optimization. Ann. Oper. Res. 37(1–4), 357–373 (1992) Nonlinear methods in economic dynamics and optimal control (Vienna, 1990)CrossRefMathSciNetMATHGoogle Scholar
  16. 16.
    Veeraklaew, T., Agrawal, S.K.: New computational framework for trajectory optimization of high-order dynamic systems. J. Guid. Control Dyn. 24(2), 228–236 (2001)Google Scholar
  17. 17.
    Cervantes, A.M., Biegler, L.: Optimization strategies for dynamic systems. In: Encyclopedia of Optimization, vol. 4, pp. 216–227. Kluwer, Norwell, MA (2001)Google Scholar
  18. 18.
    Betts, J.T.: Survey of numerical methods for trajectory optimization. J. Guid. Control Dyn. 21(2), 193–207 (1998)CrossRefGoogle Scholar
  19. 19.
    Biegler, L., Cervantes, A.M., Wächter, A.: Advances in simultaneous strategies for dynamic process optimization. Chem. Eng. Sci. 57(4), 575–593 (2002)CrossRefGoogle Scholar
  20. 20.
    Bertolazzi, E., Biral, F., Da Lio, M.: Symbolic-numeric indirect method for solving optimal control problems for large multibody, the racing vehicle example. J. Multibody Syst. Dyn. 13, 233–252 (2005)CrossRefMATHGoogle Scholar
  21. 21.
    Bertolazzi, E., Biral, F., Da Lio, M.: Symbolic-numeric efficient solution of optimal control problems for multibody systems. J. Comput. Appl. Math. 185(2), 404–421 (2006)CrossRefMathSciNetMATHGoogle Scholar
  22. 22.
    Dennis, Jr., J.E., Schnabel, R.B.: Numerical methods for unconstrained optimization and nonlinear equations. In: Classics in Applied Mathematics, vol. 16. Society for Industrial and Applied Mathematics (SIAM), Philadelphia, PA (1996) [Corrected reprint of the 1983 original]Google Scholar
  23. 23.
    Gould, N., Orban, D., Toint, P.: Numerical methods for large-scale nonlinear optimization. Acta Numer. 14, 299–361 (2005)CrossRefMathSciNetMATHGoogle Scholar
  24. 24.
    Nocedal, J., Wright, S.J.: Numerical optimization. In: Springer Series in Operations Research. Springer-Verlag, New York (1999)Google Scholar
  25. 25.
    Bouaricha, A., Schnabel, R.B.: Tensor methods for large sparse systems of nonlinear equations. Math. Program. A 82(3), 377–400 (1998)CrossRefMathSciNetGoogle Scholar
  26. 26.
    Bader, B.W.: Tensor-Krylov methods for solving large-scale systems of nonlinear equations. SIAM J. Numer. Anal. 43(3), 1321–1347 (2005) (electronic)CrossRefMathSciNetMATHGoogle Scholar
  27. 27.
    Schnabel, R.B., Frank, P.D.: Tensor methods for nonlinear equations. SIAM J. Numer. Anal. 21(5), 815–843 (1984)CrossRefMathSciNetMATHGoogle Scholar
  28. 28.
    Potra, F.A., Shi, Y.: Efficient line search algorithm for unconstrained optimization. J. Optim. Theory Appl. 85(3), 677–704 (1995)CrossRefMathSciNetMATHGoogle Scholar
  29. 29.
    Shi, Z.J., Shen, J.: Convergence of descent method with new line search. J. Appl. Math. Comput. 20(1–2), 239–254 (2006)CrossRefMathSciNetMATHGoogle Scholar
  30. 30.
    Deuflhard, P.: Newton methods for nonlinear problems. In: Springer Series in Computational Mathematics, vol. 35, Springer-Verlag, Berlin (2004)Google Scholar
  31. 31.
    Deuflhard, P., Heindl, G.: Affine invariant convergence theorems for Newton's method and extensions to related methods. SIAM J. Numer. Anal. 16(1), 1–10 (1979)CrossRefMathSciNetMATHGoogle Scholar
  32. 32.
    Grippo, L., Lampariello, F., Lucidi, S.: A nonmonotone line search technique for Newton's method. SIAM J. Numer. Anal. 23(4), 707–716 (1986)CrossRefMathSciNetMATHGoogle Scholar
  33. 33.
    Amodio, P., Cash, J.R., Roussos, G., Wright, R.W., Fairweather, G., Gladwell, I., Kraut, G.L., Paprzycki, M.: Almost block diagonal linear systems: sequential and parallel solution techniques, and applications. Numer. Linear Algebra Appl. 7(5), 275–317 (2000)CrossRefMathSciNetMATHGoogle Scholar
  34. 34.
    Amodio, P., Paprzycki, M.: A cyclic reduction approach to the numerical solution of boundary value ODEs. SIAM J. Sci. Comput. 18(1), 56–68 (1997)CrossRefMathSciNetMATHGoogle Scholar
  35. 35.
    Paprzychi, M., Gladwell, I.: Using level 3 BLAS to solve almost block diagonal systems. In: Parallel Processing for Scientific Computing, Houston, TX, 1991, pp. 52–62. SIAM, Philadelphia, PA (1992)Google Scholar
  36. 36.
    Díaz, J.C., Keast, G., Keast, P.: Algorithm 603: Colrow and arceco: Fortran packages for solving certain almost block diagonal linear systems by modified alternate row and column elimination. ACM Trans. Math. Softw. (TOMS) 9(3), 376–380 (1983)CrossRefGoogle Scholar
  37. 37.
    Brown, P.N., Saad, Y.: Hybrid Krylov methods for nonlinear systems of equations. SIAM J. Sci. Statist. Comput. 11(3), 450–481 (1990)CrossRefMathSciNetMATHGoogle Scholar
  38. 38.
    Bellavia, S., Morini, B.: A globally convergent Newton—GMRES subspace method for systems of nonlinear equations. SIAM J. Sci. Comput. 23(3), 940–960 (2001) (electronic)CrossRefMathSciNetMATHGoogle Scholar
  39. 39.
    Broyden, C.G., Dennis, Jr., J.E., Moré, J.J.: On the local and superlinear convergence of quasi-Newton methods. J. Inst. Math. Appl. 12, 223–245 (1973)CrossRefMathSciNetMATHGoogle Scholar
  40. 40.
    Gay, D.M.: Some convergence properties of Broyden's method. SIAM J. Numer. Anal. 16(4), 623–630 (1979)CrossRefMathSciNetMATHGoogle Scholar

Copyright information

© Springer Science+Business Media, Inc. 2007

Authors and Affiliations

  • Enrico Bertolazzi
    • 1
  • Francesco Biral
    • 1
  • Mauro Da Lio
    • 1
  1. 1.Department of Mechanical and Structural EngineeringUniversity of TrentoTrentoItaly

Personalised recommendations