Many applications demand a dynamical system to reach a goal state under kinematic and dynamic (i.e., kinodynamic) constraints. Moreover, industrial robots perform such motions over and over again and therefore demand efficiency, i.e., optimal motion. In many applications, the initial state may not be constrained and can be taken as an additional variable for optimization. The semi-stochastic kinodynamic planning (SKIP) algorithm presented in this paper is a novel method for trajectory optimization of a fully actuated dynamic system to reach a goal state under kinodynamic constraints. The basic principle of the algorithm is the parameterization of the motion trajectory to a vector in a high-dimensional space. The kinematic and dynamic constraints are formulated in terms of time and the trajectory parameters vector. That is, the constraints define a time-varying domain in the high dimensional parameters space. We propose a semi stochastic technique that finds a feasible set of parameters satisfying the constraints within the time interval dedicated to task completion. The algorithm chooses the optimal solution based on a given cost function. Statistical analysis shows the probability to find a solution if one exists. For simulations, we found a time-optimal trajectory for a 6R manipulator to hit a disk in a desired state.
This is a preview of subscription content, log in to check access.
Buy single article
Instant access to the full article PDF.
Price includes VAT for USA
Subscribe to journal
Immediate online access to all issues from 2019. Subscription will auto renew annually.
This is the net price. Taxes to be calculated in checkout.
Matlab is a registered trademark of The Mathworks, Inc.
Boyd, S., & Vandenberghe, L. (2004). Convex optimization. Berichte über verteilte messysteme. Cambridge: Cambridge University Press.
Brooks, R. A., & Lozano-Perez, T. (1983). A subdivision algorithm configuration space for findpath with rotation. In Proceedings of the international joint conferences on artificial intelligence (Vol. 2, pp. 799–806).
Byravan, A., Boots, B., Srinivasa, S., & Fox, D. (2014). Space-time functional gradient optimization for motion planning. In IEEE international conference on robotics and automation (ICRA)
Canny, J. (1988). The complexity of robot motion planning. ACM doctoral dissertation award. Cambridge: MIT Press.
Cauchy, A. (1847). Méthode générale pour la résolution des systémes d’équations simultanées. Comptes rendus de l’Académie des Sciences, 25, 536–538.
Clark, C. M. (2005). Probabilistic road map sampling strategies for multi-robot motion planning. Robotics and Autonomous Systems, 53(3–4), 244–264.
Denny, J., Shi, K., & Amato, N. (2013). Lazy toggle PRM: A single-query approach to motion planning. In Proceedings of the IEEE international conference on robotics and automation (pp. 2407–2414)
Donald, B., Xavier, P., Canny, J., & Reif, J. (1993). Kinodynamic motion planning. Journal of the ACM, 40(5), 1048–1066.
Dragan, A., Ratliff, N., & Srinivasa, S. (2011). Manipulation planning with goal sets using constrained trajectory optimization. In IEEE international conference on robotics and automation (ICRA) (pp. 4582–4588)
Heo, Y. J., & Chung, W. K. (2013). RRT-based path planning with kinematic constraints of AUV in underwater structured environment. In 10th international conference on ubiquitous robots and ambient intelligence (URAI) (pp. 523–525)
Hsu, D., Kindel, R., Latombe, J.-C., & Rock, S. (2002). Randomized kinodynamic motion planning with moving obstacles. The International Journal of Robotics Research, 21(3), 233–255.
Kalakrishnan, M., Chitta, S., Theodorou, E., Pastor, P., Schaal, S. (2011). Stomp: Stochastic trajectory optimization for motion planning. In 2011 IEEE international conference on robotics and automation (ICRA) (pp. 4569–4574)
Karaman, S., & Frazzoli, E. (2011). Sampling-based algorithms for optimal motion planning. The International Journal of Robotics Research, 30(7), 846–894.
Kavraki, L., Svestka, P., Latombe, J. C., & Overmars, M. (1996). Probabilistic roadmaps for path planning in high-dimensional configuration spaces. IEEE Transactions on Robotics and Automation, 12(4), 566–580.
Keller, H. (1976). Numerical solution of two point boundary value problems. Society for industrial and applied mathematics.
Khatib, O. (1986). Real-time obstacle avoidance for manipulators and mobile robots. The International Journal of Robotics Research, 5(1), 90–98.
Kim, D. H., Lim, S. J., Lee, D. H., Lee, J. Y., & Han, C. S. (2013) A RRT-based motion planning of dual-arm robot for (dis)assembly tasks. In 44th international symposium on robotics (ISR) (pp. 1–6)
Kuhn, H. W., & Tucker, A. W. (1950). Nonlinear programming. In J. Neyman (Ed.), Proceedings of the 2nd Berkeley symposium on mathematical statistics and probability (pp. 481–492). Berkeley: University of California Press.
LaValle, S., & Kuffner J. J. J. (1999). Randomized kinodynamic planning. In Proceedings of the IEEE international conference on robotics and automation (Vol. 1, pp. 473–479)
Lavalle, S. M. (1998). Rapidly-exploring random trees: A new tool for path planning. Technical report.
Lebesgue, H., & May, K. (1966). Measure and the integral., The Mathesis Series San Francisco: Holden-Day.
Luders, B., Karaman, S., Frazzoli, E., & How, J. (2010). Bounds on tracking error using closed-loop rapidly-exploring random trees. In American control conference (ACC) (pp. 5406–5412)
Motonaka, K., Watanabe, K., & Maeyama, S. (2013). Motion planning of a UAV using a kinodynamic motion planning method. In 39th annual conference of the IEEE industrial electronics society (IECON) (pp. 6383–6387)
Park, C., Pan, J., & Manocha, D. (2012). ITOMP: Incremental trajectory optimization for real-time replanning in dynamic environments. In Proceedings of the 22nd international conference on automated planning and scheduling, ICAPS 2012 (pp. 207–215), Atibaia, São Paulo, June 25–19, 2012
Pham, Q.-C., Caron, S., & Nakamura, Y. (2013). Kinodynamic planning in the configuration space via admissible velocity propagation. In Proceedings of robotics: Science and systems.
Rao, A. (2014). Trajectory optimization: A survey. In H. Waschl, I. Kolmanovsky, M. Steinbuch, & L. del Re (Eds.), Optimization and optimal control in automotive systems (Vol. 455, pp. 3–21)., Lecture notes in control and information sciences Berlin: Springer.
Rao, A. V. (2009). A survey of numerical methods for optimal control. Advances in the Astronautical Sciences, 135(1), 497–528.
Ratliff, N., Zucker, M., Bagnell, J. A. D., & Srinivasa, S. (2009). Chomp: Gradient optimization techniques for efficient motion planning. In IEEE international conference on robotics and automation (ICRA)
Reif, J. (1979). Complexity of the mover’s problem and generalizations. In 20th annual symposium on foundations of computer science (pp. 421–427)
Rimon, E., & Koditschek, D. (1992). Exact robot navigation using artificial potential functions. IEEE Transactions on Robotics and Automation, 8(5), 501–518.
Schulman, J., Ho, J., Lee, A., Awwal, I., Bradlow, H., & Abbeel, P. (2013). Finding locally optimal, collision-free trajectories with sequential convex optimization. In Proceedings of the robotics: Science and systems (RSS)
Schwartz, J. T., & Sharir, M. (1983). On the ’piano movers’ problem. II. General techniques for computing topological properties of real algebraic manifolds. Advances in Applied Mathematics, 4(3), 298–351.
Sintov, A., & Shapiro, A. (2014) Time-based RRT algorithm for rendezvous planning of two dynamic systems. In Proceedings of the IEEE international conference on robotics and automation (ICRA) (pp. 6745–6750)
Sintov, A., & Shapiro, A. (2015). A stochastic dynamic motion planning algorithm for object-throwing. In Proceedings of the IEEE international conference on robotics and automation
Sintov, A., & Shapiro, A. (2017). Dynamic regrasping by in-hand orienting of grasped objects using non-dexterous robotic grippers. In Robotics and computer-integrated manufacturing
Song, G., & Amato, N. (2001). Randomized motion planning for car-like robots with c-prm. In Proceedings of the IEEE/RSJ international conference on intelligent robots and systems (Vol. 1, pp. 37–42)
Spong, M. W., Hutchinson, S., & Vidyasagar, M. (2006). Robot modeling and control (Vol. 3). New York: Wiley.
Van der Stappen, A., Halperin, D., & Overmars, M. (1993). Efficient algorithms for exact motion planning amidst fat obstacles. In Proceedings of the IEEE international conference on robotics and automation (Vol. 1, pp. 297–304)
Van der Stappen, A. F., Overmars, M. H., de Berg, M., & Vleugels, J. (1998). Motion planning in environments with low obstacle density. Discrete & Computational Geometry, 20(4), 561–587.
Verscheure, D., Demeulenaere, B., Swevers, J., De Schutter, J., & Diehl, M. (2009). Time-optimal path tracking for robots: A convex optimization approach. IEEE Transactions on Automatic Control, 54(10), 2318–2327.
Webb, D., & van den Berg, J. (2013). Kinodynamic rrt*: Asymptotically optimal motion planning for robots with linear dynamics. In: IEEE international conference on robotics and automation (ICRA) (pp. 5054–5061)
Zucker, M., Ratliff, N., Dragan, A. D., Pivtoraiko, M., Klingensmith, M., Dellin, C. M., et al. (2013). CHOMP: Covariant Hamiltonian optimization for motion planning. The International Journal of Robotics Research, 32(9–10), 1164–1193.
The research was supported by the Helmsley Charitable Trust through the Agricultural, Biological and Cognitive Robotics Center of Ben-Gurion University of the Negev.
Electronic supplementary material
Below is the link to the electronic supplementary material.
About this article
Cite this article
Sintov, A. Goal state driven trajectory optimization. Auton Robot 43, 631–648 (2019). https://doi.org/10.1007/s10514-018-9728-3
- Motion planning
- Kinodynamic constraints
- Trajectory optimization