Optimization and Engineering

, Volume 17, Issue 3, pp 533–556 | Cite as

Combining discrete and continuous optimization to solve kinodynamic motion planning problems

  • Chantal Landry
  • Wolfgang Welz
  • Matthias Gerdts


A new approach to find the fastest trajectory of a robot avoiding obstacles, is presented. This optimal trajectory is the solution of an optimal control problem with kinematic and dynamic constraints. The approach involves a direct method based on the time discretization of the control variable. We mainly focus on the computation of a good initial trajectory. Our method combines discrete and continuous optimization concepts. First, a graph search algorithm is used to determine a list of intermediate points. Then, an optimal control problem of small size is defined to find the fastest trajectory that passes through the vicinity of the intermediate points. The resulting solution is the initial trajectory. Our approach is applied to a single body mobile robot. The numerical results show the quality of the initial trajectory and its low computational cost.


Trajectory planning Optimal control problem Collision avoidance Graph search algorithm Initialization Robotics 

Mathematics Subject Classification

49J15 49M25 49N90 70E60 90C30 90C35 


  1. Barclay A, Gill PE, Ben Rosen J (1997) SQP methods and their application to numerical optimal control. University of California, San DiegoMATHGoogle Scholar
  2. Berkovitz LD (2001) Convexity and optimization in \(\mathbb{R}^{n}\). Wiley, New YorkGoogle Scholar
  3. Betts JT (2001) Practical methods for optimal control using nonlinear programming. Advances in design and control. Society for Industrial and Applied Mathematics (SIAM), PhiladelphiaMATHGoogle Scholar
  4. Bobrow JE (1988) Optimal robot path planning using the minimum-time criterion. IEEE J Robot Autom 4:443–450CrossRefGoogle Scholar
  5. Bobrow JE, Dubowsky S, Gibson JS (1985) Time-optimal control of robotic manipulators along specified paths. Int J Robot Res 4:3–17CrossRefGoogle Scholar
  6. Bohlin R (2002) Robot path planning. Chalmers University of Technology, GoteborgGoogle Scholar
  7. Cameron SA, Culley RK (1986) Determining the minimum translational distance between two convex polyhedra. In: Proceedings of international conference on robotics and automation, pp 591–596Google Scholar
  8. Diehl M, Walther A, Bock HG, Kostina E (2010) An adjoint-based SQP algorithm with quasi-Newton Jacobian updates for inequality constrained optimization. Optim Methods Softw 25(4–6):531–552MathSciNetCrossRefMATHGoogle Scholar
  9. Donald B, Xavier P, Canny J, Reif J (1993) Kinodynamic motion planning. J Assoc Comput Mach 40:1048–1066MathSciNetCrossRefMATHGoogle Scholar
  10. Dubowsky S, Norris MA, Shiller Z (1989) Time optimal trajectory planning for robotic manipulators with obstacle avoidance: a CAD approach. In: Proceedings of IEEE international conference on robotics and automation, pp 1906–1912Google Scholar
  11. Escande A, Miossec S, Benallegue M, Kheddar A (2014) A strictly convex hull for computing proximity distances with continuous gradients. IEEE Trans Robot 30(3):666–678CrossRefGoogle Scholar
  12. Fletcher R, Leyffer S (2002) Nonlinear programming without a penalty function. Math Program 91(2):239–269MathSciNetCrossRefMATHGoogle Scholar
  13. Fletcher R, Leyffer S, Toint P (2002) On the global convergence of a filter-SQP algorithm. SIAM J Optim 13(1):44–59MathSciNetCrossRefMATHGoogle Scholar
  14. Gerdts M (2013) OCPID-DAE1: optimal control and parameter identification with differential-algebraic equations of index 1, User Manual, Version 1.3, Department of Aerospace Engineering, Universität der Bundeswehr München.
  15. Gerdts M (2012) Optimal Control of ODEs and DAEs. De Gruyter, De Gruyter textbookGoogle Scholar
  16. Gerdts M, Henrion R, Hömberg D, Landry C (2012) Path planning and collision avoidance for robots. Numer Algebra Control Optim 2(3):437–463MathSciNetCrossRefMATHGoogle Scholar
  17. Gilbert EG, Johnson DW (1985) Distance functions and their application to robot path planning in the presence of obstacles. IEEE J Robot Autom RA–1:21–30CrossRefGoogle Scholar
  18. Gilbert EG, Johnson DW, Keerthi SS (1988) A fast procedure for computing the distance between complex objects in three-dimensional space. IEEE J Robot Autom 4(2):193–203CrossRefGoogle Scholar
  19. Goerzen C, Kong Z, Mettler B (2010) A survey of motion planning algorithms from the perspective of autonomous UAV guidance. J Intell Robot Syst 57(1–4):65–100CrossRefMATHGoogle Scholar
  20. Hart GD, Anitescu M (2010) An O(m + n) measure of penetration depth between convex polyhedral bodies for rigid multibody dynamicsGoogle Scholar
  21. Johnson DW, Gilbert EG (1985) Minimum time robot path planning in the presence of obstacles, vol 24. In: 24th IEEE conference on decision and control, pp 1748–1753Google Scholar
  22. Kim YJ, Lin MC, Manocha D (2002) DEEP: dual-space expansion for estimating penetration depth between convex polytopes. In: IEEE conference on robotics and automation, pp 921–926Google Scholar
  23. Landry C, Gerdts M, Henrion R, Hömberg D (2013) Path-planning with collision avoidance in automotive industry. In: 25th IFIP TC 7 conference system modeling and optimization, Berlin. 12–16 Sep 2011. Revised Selected Papers IFIP AICT 391, Springer, Heidelberg; Approx. IX, 575 pp 2013Google Scholar
  24. LaValle SM (2006) Planning algorithms. Cambridge University Press, CambridgeCrossRefMATHGoogle Scholar
  25. LaValle SM, Kuffner JJ (2001) Randomized kinodynamic planning. Int J Robot Res 20(5):378–400CrossRefGoogle Scholar
  26. Lewis AS, Overton ML (2008) Nonsmooth optimization via BFGS. Technical report, papers/pdffiles/bfgs_inexactLS.pdf
  27. Lewis AS, Overton ML (2013) Nonsmooth optimization via quasi Newton methods. Math Progr 141(1–2):135–163MathSciNetCrossRefMATHGoogle Scholar
  28. Lin MC (1993) Efficient collision detection for animation and robotics. PhD thesis, Department of Electrical Engineering and Computer Science, University of California, BerkeleyGoogle Scholar
  29. Lin MC, Canny JF (1991) A fast algorithm for incremental distance calculation. In: IEEE international conference on robotics and automation, pp 1008–1014Google Scholar
  30. Lin Q, Loxton RC, Teo KL (2014) The control parameterization method for nonlinear optimal control: a survey. J Ind Manag Optim 10(1):275–309MathSciNetCrossRefMATHGoogle Scholar
  31. Loxton RC, Teo KL, Rehbock V, Yiu KFC (2009) Optimal control problems with a continuous inequality constraint on the state and the control. Automatica 45(10):2250–2257MathSciNetCrossRefMATHGoogle Scholar
  32. Maheshwari A, Sack JR, Djidjev HN (1999) Link distance problems. Handbook of computational geometry, pp 519–558Google Scholar
  33. Nocedal J, Wright SJ (2006) Numerical optimization, 2nd edn., Springer series in operations research and financial engineering. Springer, New YorkMATHGoogle Scholar
  34. Powell MJD (1978) A fast algorithm for nonlinearly constrained optimization calculations. In: Watson GA (ed) Numerical analysis, vol 630. Lecture notes in mathematics. Springer, Berlin, pp 144–157Google Scholar
  35. Quarteroni A, Sacco R, Saleri F (2007) Numerical mathematics., Texts in applied mathematics. Springer, ParisCrossRefMATHGoogle Scholar
  36. Saramago SFP, Steffen V (2001) Trajectory modeling of robot manipulators in the presence of obstacles. J Optim Theory Appl 110:17–34MathSciNetCrossRefMATHGoogle Scholar
  37. Schittkowski K (1983) On the convergence of a sequential quadratic programming method with an augmented Lagrangian line search function 2. Mathematische Operationsforschung und Statistik. Series Optimization 14(2):197–216MathSciNetCrossRefMATHGoogle Scholar
  38. Schramm H, Zowe J (1992) A version of the bundle idea for minimizing a nonsmooth function: conceptual idea, convergence analysis, numerical results. SIAM J Optim 2(1):121–152MathSciNetCrossRefMATHGoogle Scholar
  39. Shor NZ (1985) Minimization methods for non-differentiable functions, vol 3., Springer series in computational mathematics. Springer, BerlinMATHGoogle Scholar
  40. Sprunk C, Lau B, Pfaffz P, Burgard W (2011) Online generation of kinodynamic trajectories for non-circular omnidirectional robots. In: IEEE international conference on Robotics and automation (ICRA), pp 72–77Google Scholar
  41. Winter S (2002) Modeling costs of turns in route planning. GeoInformatica 6(4):345–361CrossRefMATHGoogle Scholar

Copyright information

© Springer Science+Business Media New York 2015

Authors and Affiliations

  • Chantal Landry
    • 1
  • Wolfgang Welz
    • 2
  • Matthias Gerdts
    • 3
  1. 1.Zurich University of Applied SciencesWinterthurSwitzerland
  2. 2.Technische Universität BerlinBerlinGermany
  3. 3.University of the Federal Armed Forces at MunichNeubibergGermany

Personalised recommendations