Abstract
Integrated motion planning and control for the purposes of maneuvering mobile robots under state- and input constraints is a problem of vital practical importance in applications of mobile robots such as autonomous transportation. Those constraints arise naturally in practice due to specifics of robot mechanical construction and the presence of obstacles in motion environment. In contrast to approaches focusing on feedback control design under the assumption of given reference motion or motion planning with neglection of subsequent feedback motion execution, we adopt a controller-driven motion planning paradigm, which has recently gained attention of many researchers. It postulates design of motion planning algorithms dedicated to specific feedback control policies, which compute a sequence of feedback control subtasks instead of classically planned open-loop controls or parametric paths. In this spirit, we propose a motion planning algorithm driven by the VFO (Vector Field Orientation) control law for the waypoint-following task. Presented analysis of the VFO control law reveals its beneficial properties, which are subsequently utilized to solve a generally nonlinear and non-convex optimal motion planning problem by formulating it as a mixed-integer linear program (MILP). The solution proposed in this paper yields a waypoint sequence, which is designed for execution by application of the VFO control law to drive a robot to a prescribed final configuration under an input constraint imposed by bounded curvature of robot motion and state constraints resulting from a convex decomposition of task space. Satisfaction of these constraints is guaranteed analytically and exactly, i.e., without utilization of numerical approximations. Moreover, for a given discrete set of possible waypoint orientations, the proposed algorithm computes plans optimal w.r.t. given cost functional, which can be any convex linear combination of quantities such as robot path length, curvature of robot motion, distance to imposed state constraints, etc. Furthermore, the planning algorithm exploits the possibility of both forward or backward movement of the robot to allow maneuvering in demanding environments. Generated waypoint sequences are a compact representation of a motion plan, which can be immediately executed with the VFO controller without any additional post-processing. Validity of the proposed approach has been confirmed by simulation studies and experimental motion execution with a laboratory-scale mobile robot.
Article PDF
Similar content being viewed by others
Avoid common mistakes on your manuscript.
References
Bakolas, E., Tsiotras, P.: On the generation of nearly optimal, planar paths of bounded curvature and bounded curvature gradient In: American Control Conference ACC ’09, pp 385–390, St. Louis, Missouri, USA (2009)
Bicchi, A., Casalino, G., Santilli, C.: Planning shortest bounded-curvature paths for a class of nonholonomic vehicles among obstacles. J. Intell. Robot. Syst. 16(4), 387–405 (1996)
Blackmore, L.: Robust path planning and feedback design under stochastic uncertainty In: Proceedings of the AIAA Guidance, Navigation and Control Conference, Honolulu, Hawaii (2008)
Chen, C., Rickert, M., Knoll, A.: Combining space exploration and heuristic search in online motion planning for nonholonomic vehicles In: Proceedings of the IEEE Intelligent Vehicles Symposium, pp 1307–1312, Gold Coast, Australia (2013)
Conner, D.C., Choset, H., Rizzi, A.A.: Integrating planning and control for single-bodied wheeled mobile robots. Auton. Robots 30(3), 243–264 (2011)
Dubins, L.E.: On curves of minimal length with a constraint on average curvature, and with prescribed initial and terminal positions and tangents. Am. J. Math. 79(3), 497–516 (1957)
Su Francis, E., et al.: Zero to the zero power. https://www.math.hmc.edu/funfacts/ffiles/10005.3-5.shtml. Accessed: 2016-06-20
Michałek, M., Kozłowski, K.: Vector-Field-Orientation feedback control method for a differentially driven vehicle. IEEE Trans. Control Syst. Technol. 18(1), 45–65 (2010)
Elbanhawi, M., Simic, M., Jazar, R.: Continuous path smoothing for car-like robots using B-spline curves. J. Intell. Robot. Syst., 1–34 (2015)
Fraichard, T., Scheuer, A.: From Reeds and Shepp’s to continuous-curvature paths. IEEE Trans. on Robotics 20(6), 1025–1035 (2004)
Gammell, J.D., Srinivasa, S.S., Barfoot, T.D.: Batch informed trees (BIT*): Sampling-based optimal planning via the heuristically guided search of implicit random geometric graphs In: 2015 IEEE International Conference on Robotics and Automation (ICRA), pp 3067–3074, Seattle, Washington, USA (2015)
Gawron, T., Michałek, M.M.: VFO stabilization of a unicycle robot with bounded curvature of motion In: International Workshop on Robot Motion and Control, p 2015, Poznań, Poland (2015)
Haddad, M., Khalil, W., Lehtihet, H.E.: Trajectory planning of unicycle mobile robots with a trapezoidal-velocity constraint. IEEE Trans. on Robotics 26(5), 954–962 (2010)
Hale, D.H., Youngblood, G.M., Dixit, P.N.: Automatically-generated convex region decomposition for real-time spatial agent navigation in virtual worlds In: AIIDE, Stanford, California, USA (2008)
Jaillet, L., Hoffman, J., van den Berg, J., Abbeel, P., Porta, J. M., Goldberg, K.: EG-RRT Environment-guided random trees for kinodynamic motion planning with uncertainty and obstacles In: 2011 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp 2646–2652 (2011)
Janiak, M., Tchoń, K.: Constrained motion planning of nonholonomic systems. Sys. Control Lett. 60(8), 625–631 (2011)
Janson, L., Schmerling, E., Clark, A., Pavone, M.: Fast marching tree: a fast marching sampling-based method for optimal motion planning in many dimensions. Int. J. Rob. Res. 34(7), 883–921 (2015)
Hwan Jeon, J., Cowlagi, R.V., Peters, S.C., Karaman, S., Frazzoli, E., Tsiotras, P., Iagnemma, K.: Optimal motion planning with the half-car dynamical model for autonomous high-speed driving In: American Control Conference (ACC), 2013, pp 188–193, Washington, DC, USA (2013)
Afonso, R.J.M., Galvao, R.K.H., Kienitz, K.H.: Waypoint trajectory planning in the presence of obstacles with a tunnel-MILP approach, Zürich, Switzerland (2013)
Karaman, S., Frazzoli, E.: Sampling-based optimal motion planning for non-holonomic dynamical systems, Karlsruhe, Germany (2013)
Karaman, S., Walter, M.R., Perez, A., Frazzoli, E., Teller, S.: Anytime motion planning using the RRT*, Shanghai, China (2011)
Knepper, R.A., Mason, M.T.: Real-time informed path sampling for motion planning search. Int. J. Rob. Res. 31(11), 1231–1250 (2012)
Kuwata, Y., Fiore, G.A., Teo, J., Frazzoli, E., How, J.P.: Motion planning for urban driving using rrt, Nice, France (2008)
Lamiraux, F., Bonnafous, D., Lefebvre, O.: Reactive path deformation for nonholonomic mobile robots. IEEE Trans. on Robotics 20(6), 967–977 (2004)
Landry, B., Deits, R., Florence, P.R., Tedrake, R.: Aggressive quadrotor flight through cluttered environments using mixed integer programming, Stockholm, Sweden (2016)
Latombe, J.-C.: Robot Motion Planning. Kluwer Academic Publishers, Norwell (1991)
Lau, B., Sprunk, C., Burgard, W.: Kinodynamic motion planning for mobile robots using splines In: Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp 2427–2433, St. Louis, Missouri, USA (2009)
LaValle, S.M.: Planning algorithms. Cambridge University Press, Cambridge, U.K. (2006)
LaValle, S.M.: Motion planning: The wild frontiers. IEEE Robot. Autom. Mag. 18(2), 108–118 (2011)
Li, Yanbo, Littlefield, Z., Bekris, K.E.: Asymptotically optimal sampling-based kinodynamic planning. Int. J. Rob. Res. 35(5), 528–564 (2016)
Likhachev, M., Ferguson, D., Gordon, G., Stentz, A. (Tony), Thrun, S.: Anytime dynamic A*: An anytime, replanning algorithm In: Proceedings of the International Conference on Automated Planning and Scheduling (ICAPS) (2005)
Lini, G., Piazzi, A., Consolini, L.: Multi-optimization of η 3-splines for autonomous parking In: Decision and Control and European Control Conference (CDC-ECC) 2011, pp 6367–6372, Orlando, Florida, USA (2011)
Michałek, M., Kiełczewski, M., Jedwabny, T.: Cascaded VFO control for non-standard N-trailer robots. J. Intell. Rob. Syst. 77(3), 415–432 (2015). doi:10.1007/s10846-013-9881-z
Michałek, M., Kozłowski, K.: Motion planning and feedback control for a unicycle in a way point following task: the VFO approach. Int. J. Appl. Math. Comput. Sci. 19(4), 533–545 (2009)
Michałek, M., Kozłowski, K.: Feedback control framework for car-like robots using the unicycle controllers. Robotica 30, 517–535 (2012)
Morin, P., Samson, C.: Motion Control of Wheeled Mobile Robots, pp 799–826. Springer, Heidelberg, Berlin (2008)
Narayanan, V., Phillips, M., Likhachev, M.: Anytime safe interval path planning for dynamic environments (2012)
Palmieri, L., Arras, K.O.: A novel RRT extend function for efficient and smooth mobile robot motion planning In: 2014 IEEE/RSJ International Conference on Intelligent Robots and Systems, pp 205–211, Chicago, Illinois, USA (2014)
Park, J.J., Kuipers, B.: Feedback motion planning via non-holonomic RRT for mobile robots In: Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp 4035–4040, Hamburg, Germany (2015)
Perez, A., Platt, R., Konidaris, G., et al.: LQR-RRT*: Optimal sampling-based motion planning with automatically derived extension heuristics In: 2012 IEEE International Conference Robotics and Automation, pp 2537–2542, Saint Paul, MN (2012)
Piazzi, A., Bianco, C.G.L., Romano, M.: η 3 splines for the smooth path generation of wheeled mobile robots. IEEE Trans. on Robotics 23(5), 1089–1095 (2007)
Pivtoraiko, M., Knepper, R.A., Kelly, A.: Differentially constrained mobile robot motion planning in state lattices. J. Field Rob. 26(3), 308–333 (2009)
Reeds, J.A., Shepp, R.A.: Optimal paths for a car that goes both forward and backwards. Pac. J Mathematics 145(2), 367–393 (1990)
Reist, P., Preiswerk, P., Tedrake, R.: Feedback-motion-planning with simulation-based LQR-trees. Int. J. Rob. Res. 35(11), 1393–1416 (2016)
Rickert, M., Brock, O., Knoll, A.: Balancing exploration and exploitation in motion planning In: Proceedings of the IEEE International Conference on Robotics and Automation, pp 2812–2817, Pasadena, California, USA (2008)
Schouwenaars, T., How, J., Feron, E.: Receding horizon path planning with implicit safety guarantees In: Proceedings of the American Control Conference, vol. 6, p 2004, Boston, Massachusetts (2004)
Seidel, R.: A simple and fast incremental randomized algorithm for computing trapezoidal decompositions and for triangulating polygons. Comput. Geom. Theory Appl. 1(1), 51–64 (1991)
Shiller, Z., Gwo, Y.R.: Dynamic motion planning of autonomous vehicles. IEEE Trans. Robot. Autom. 7(2), 241–249 (1991)
Shiller, Z., Sharma, S., Stern, I., Stern, A.: Online obstacle avoidance at high speeds. Int. J. Rob. Res. 32(9-10), 1030–1047 (2013)
Tokekar, P., Karnad, N., Isler, V.: Energy-optimal trajectory planning for car-like robots. Auton. Robots. 37(3), 279–300 (2014)
van Duijkeren, N., Nilsson, P., Laine, L., Keviczky, T.: Real-time NMPC for semi-automated highway driving of long heavy vehicle combinations In: Proceedings 5h IFAC Conference on Nonlinear Model Predictive Control 2015, pp 39–46, Seville, Spain (2015)
Velenis, E., Tsiotras, P.: Minimum-time travel for a vehicle with acceleration limits: Theoretical analysis and receding-horizon implementation. J. Optim. Theory Appl. 138(2), 275–296 (2008)
Vitus, M.P., Pradeep, V., Hoffmann, G.M., Wasl, S.L., Tomlin, C.J.: Tunnel-MILP: Path planning with sequential convex polytopes In: AIAA Guidance, Navigation, and Control Conference (2008)
Yang, K., Moon, S., Yoo, S., Kang, J., Doh, N., Kim, H., Joo, S.: Spline-based RRT path planner for non-holonomic robots. J. Intell. Robot. Syst. 73(1-4), 763–782 (2014)
Zhang, H., Butzke, J., Likhachev, M.: Combining global and local planning with guarantees on completeness (2012)
Zhu, Z., Schmerling, E., Pavone, M.: A convex optimization approach to smooth trajectories for motion planning with car-like robots In: Proceedings of the IEEE Conference on Decision and Control, Osaka, Japan (2015)
Author information
Authors and Affiliations
Corresponding author
Additional information
This work was financially supported by the National Science Centre (NCN), Poland, as the research grant No. 2016/21/B/ST7/02259.
Rights and permissions
Open Access This article is distributed under the terms of the Creative Commons Attribution 4.0 International License (http://creativecommons.org/licenses/by/4.0/), which permits unrestricted use, distribution, and reproduction in any medium, provided you give appropriate credit to the original author(s) and the source, provide a link to the Creative Commons license, and indicate if changes were made.
About this article
Cite this article
Gawron, T., Michałek, M.M. The VFO-Driven Motion Planning and Feedback Control in Polygonal Worlds for a Unicycle with Bounded Curvature of Motion. J Intell Robot Syst 89, 265–297 (2018). https://doi.org/10.1007/s10846-017-0555-0
Received:
Accepted:
Published:
Issue Date:
DOI: https://doi.org/10.1007/s10846-017-0555-0