This paper focuses on the collision-free coordination of multiple robots with kinodynamic constraints along specified paths. We present an approach to generate continuous velocity profiles for multiple robots; these velocity profiles satisfy the dynamics constraints, avoid collisions, and minimize the completion time. The approach, which combines techniques from optimal control and mathematical programming, consists of identifying collision segments along each robot’s path, and then optimizing the robots’ velocities along the collision and collision-free segments. First, for each path segment for each robot, the minimum and maximum possible traversal times that satisfy the dynamics constraints are computed by solving the corresponding two-point boundary value problems. The collision avoidance constraints for pairs of robots can then be combined to formulate a mixed integer nonlinear programming (MINLP) problem. Since this nonconvex MINLP model is difficult to solve, we describe two related mixed integer linear programming (MILP) formulations, which provide schedules that give lower and upper bounds on the optimum; the upper bound schedule is designed to provide continuous velocity trajectories that are feasible. The approach is illustrated with coordination of multiple robots, modeled as double integrators subject to velocity and acceleration constraints. An application to coordination of nonholonomic car-like robots is described, along with implementation results for 12 robots.
Get full access to this article
View all access options for this article.
References
1.
Akella, S. and Hutchinson, S. 2002. Coordinating the motions of multiple robots with specified trajectories . Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), Washington, DC, May, pp. 624-631 .
2.
Akella, S. and Peng, J. 2004. Time-scaled coordination of multiple manipulators . Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), New Orleans, LA, April, pp. 3337-3344 .
3.
Alami, R., Fleury, S., Herrb, M., Ingrand, F., and Robert, F. 1998. Multi-robot cooperation in the MARTHA project . IEEE Robotics and Automation Magazine5(1): 36-47 .
4.
Aronov, B., de Berg, M., van der Stappen, A. F., Švestka, P., and Vleugels, J. 1999. Motion planning for multiple robots . Discrete and Computational Geometry22: 505-525 .
5.
Barraquand, J., Langlois, B., and Latombe, J.-C. 1992. Numerical potential field techniques for robot path planning . IEEE Transactions on Systems, Man, and Cybernetics22(2): 224-241 .
6.
Betts, J. T. 2001. Practical Methods for Optimal Control Using Nonlinear Programming. Society for Industrial and Applied Mathematics, New York .
7.
Bicchi, A. and Pallottino, L. 2000. On optimal cooperative conflict resolution for air traffic management systems . IEEE Transactions on Intelligent Transportation Systems1(4): 221-231 .
8.
Bien, Z. and Lee, J. 1992. A minimum time trajectory planning method for two robots . IEEE Transactions on Robotics and Automation8(3): 414-418 .
9.
Bobrow, J. E., Dubowsky, S., and Gibson, J. S. 1985. Time-optimal control of robotic manipulators along specified paths . International Journal of Robotics Research4(3): 3-17 .
10.
Bryson, A. E. and Ho, Y-C. 1975. Applied Optimal Control. Hemisphere, Washington DC .
11.
Buckley, S. J. 1989. Fast motion planning for multiple moving robots . Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), Scottsdale, AZ, May, pp. 322-326 .
12.
Canny, J., Rege, A., and Reif, J. 1991. An exact algorithm for kinodynamic planning in the plane . Discrete and Computational Geometry6: 461-484 .
13.
Chang, C., Chung, M. J., and Bien, Z. 1990. Collision-free motion planning for two articulated robot arms using minimum distance functions . Robotica8: 137-144 .
14.
Chang, C., Chung, M. J., and Lee, B. H. 1994. Collision avoidance of two robot manipulators by minimum delay time . IEEE Transactions on Systems, Man, and Cybernetics24(3): 517-522 .
15.
Donald, B. R. and Xavier, P. 1995a. Provably good approximation algorithms for optimal kinodynamic planning: Robots with decoupled dynamics bounds . Algorithmica14(6): 443-479 .
16.
Donald, B. R. and Xavier, P. 1995b. Provably good approximation algorithms for optimal kinodynamic planning for Cartesian robots and open chain manipulators . Algorithmica14(6): 480-530 .
17.
Donald, B. R., Xavier, P., Canny, J., and Reif, J. 1993. Kinodynamic motion planning . Journal of the ACM40(5): 1048-1066 .
18.
Erdmann, M. and Lozano-Perez, T. 1987. On multiple moving objects . Algorithmica2(4): 477-521 .
19.
Fiorini, P. and Shiller, Z. 1993. Motion planning in dynamic environments using the relative velocity paradigm . Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), Atlanta, GA, May, pp. 560-565 .
20.
Fiorini, P. and Shiller, Z. 1996. Time-optimal trajectory planning in dynamic environments . Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), Minneapolis, MN, April, pp. 1553-1558 .
21.
Fiorini, P. and Shiller, Z. 1998. Motion planning in dynamic environments using velocity obstacles . International Journal of Robotics Research17(7): 760-772 .
22.
Fourer, R., Gay, D. M., and Kernighan, B. W. 1993. AMPL: A Modeling Language for Mathematical Programming. Duxbury Press .
23.
Fraichard, T. 1999. Trajectory planning in dynamic workspace: a “state-time space” approach . Advanced Robotics13(1): 75-94 .
24.
Fraichard, T., Scheuer, A., and Desvigne, R. 1999. From Reeds and Shepp’s to continuous-curvature paths . Proceedings of the International Conference on Advanced Robotics, Tokyo, Japan, October, pp. 585-590 .
25.
Frazzoli, E. 2003. Quasi-random algorithms for real-time spacecraft motion planning and coordination . Acta Astronautica53(4-10): 485-495 .
26.
Frazzoli, E., Mao, Z., Oh, J., and Feron, E. 2001. Resolution of conflicts involving many aircraft via semidefinite programming . AIAA Journal of Guidance, Control and Dynamics24(1): 79-86 .
27.
Freund, E. and Hoyer, H. 1988. Real-time path finding in multirobot systems including obstacle avoidance . International Journal of Robotics Research7(1): 42-70 .
28.
Fujimura, K. and Samet, H. 1989. A hierarchical strategy for path planning among moving obstacles . IEEE Transactions on Robotics and Automation5(1): 61-69 .
29.
Ghrist, R. and Koditschek, D. 2002. Safe cooperative robot dynamics on graphs . SIAM Journal on Control and Optimization40(5): 1556-1575 .
30.
Gill, P. E., Murray, W., and Saunders, M. A. 2002. SNOPT: an SQP algorithm for large-scale constrained optimization . SIAM Journal on Optimization12(4): 979-1006 .
31.
Hao, Y., Laxton, B., Agrawal, S. K., Lee, E., and Benson, E. 2003. Planning and control of UGV formations in a dynamic environment: A practical framework with experiments . Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), Taipei, Taiwan, September, pp. 1209-1214 .
32.
Heinzinger, G., Jacobs, P., Canny, J., and Paden, B. 1990. Time-optimal trajectories for a robot manipulator: a provably good approximation algorithm . Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), Cincinnati, OH, May, pp. 150-156 .
33.
Hollerbach, J. M. 1984. Dynamic scaling of manipulator trajectories . ASME Journal of Dynamic Systems, Measurement, and Control106(1): 102-106 .
34.
Hopcroft, J. E., Schwartz, J. T., and Sharir, M. 1984. On the complexity of motion planning for multiple independent objects: PSPACE-hardness of the “warehouseman’s problem” . International Journal of Robotics Research3(4): 76-88 .
35.
Hsu, D., Kindel, R., Latombe, J.-C., and Rock, S. 2001. Randomized kinodynamic motion planning with moving obstacles. Algorithmic and Computational Robotics: New Directions, B. R. Donald, K. M. Lynch, and D. Rus, editors, A. K. Peters, Natick, MA , pp. 247-264.
36.
ILOG Inc. 1999. CPLEX 6.6 User’s Manual. Incline Village, NV .
37.
Kahn, M. E. and Roth, B. 1971. The near-minimum-time control of open-loop articulated kinematic chains . ASME Journal of Dynamic Systems, Measurement, and Control93(3): 164-172 .
38.
Kant, K. and Zucker, S. W. 1986. Toward efficient trajectory planning: The path-velocity decomposition . International Journal of Robotics Research5(3): 72-89 .
39.
Knowles, G. 1981. An Introduction to Applied Optimal Control. Mathematics in Science and Engineering, Vol. 159, Academic Press, New York .
40.
Kosecka, J., Tomlin, C., Pappas, G., and Sastry, S. 1997. Generation of conflict resolution maneuvers for air traffic management . Proceedings of the IEEE/RSJ International Conference on Robots and Systems (IROS), Grenoble, France, pp. 1598-1603 .
41.
Lamiraux, F. and Laumond, J.-P. 1998. From paths to trajectories for multi-body mobile robots . International Symposium on Experimental RoboticsV, Lecture Notes in Control and Information Sciences, A. Casals and A. T. DeAlmeida, editors, Springer-Verlag, Berlin.
42.
Lamiraux, F. and Laumond, J.-P. 2001. Smooth motion planning for car-like vehicles . IEEE Transactions on Robotics and Automation17(4): 498-502 .
43.
Larsen, E., Gottschalk, S., Lin, M., and Manocha, D. 2000. Fast distance queries using rectangular swept sphere volumes . Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), San Francisco, CA, April, pp. 3719-3726 .
Laumond, J.-P., editor. 1998. Robot Motion Planning and Control , Lecture Notes in Control and Information Sciences Vol. 229, Springer-Verlag, Berlin.
46.
LaValle, S. M. and Hutchinson, S. A. 1998. Optimal motion planning for multiple robots having independent goals . IEEE Transactions on Robotics and Automation14(6): 912-925 .
47.
LaValle, S. M. and Kuffner, J. J. 2001. Randomized kinodynamic planning . International Journal of Robotics Research20(5): 378-401 .
48.
Lee, B. H. and Lee, C. S. G. 1987. Collision-free motion planning of two robots . IEEE Transactions on Systems, Man, and Cybernetics17(1): 21-32 .
49.
Nemhauser, G. L. and Wolsey, L. A. 1988. Integer and Combinatorial Optimization, Wiley, New York .
50.
O’Donnell, P. A. and Lozano-Perez, T. 1989. Deadlock-free and collision-free coordination of two robot manipulators . Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), Scottsdale, AZ, May, pp. 484-489 .
51.
O’Dunlaing, C. 1987. Motion planning with inertial constraints . Algorithmica2(4): 431-475 .
52.
Pallottino, L., Feron, E., and Bicchi, A. 2002. Conflict resolution problems for air traffic management systems solved with mixed integer programming .IEEE Transactions on Intelligent Transportation Systems3(1): 3-11 .
53.
Parsons, D. and Canny, J. 1990. Motion planning for multiple mobile robots . Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), Cincinnati, OH, pp. 8-13 .
54.
Peng, J. and Akella, S. 2003a. Coordinating multiple robots with kinodynamic constraints along specified paths. Algorithmic Foundations of Robotics V, J.-D. Boissonnat, J. Burdick, K. Goldberg, and S. Hutchinson, editors, Springer-Verlag, Berlin , pp. 221-237.
55.
Peng, J. and Akella, S. 2003b. Coordinating the motions of multiple robots with kinodynamic constraints . Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), Taipei, Taiwan, September, pp. 4066-4073 .
56.
Pfeiffer, F. and Johanni, R. 1987. A concept for manipulator trajectory planning . IEEE Journal of Robotics and Automation3(2): 115-123 .
57.
Pinedo, M. 1995. Scheduling: Theory, Algorithms, and Systems, Prentice-Hall, Englewood Cliffs, NJ .
58.
Pledgie, S., Hao, Y., Ferreira, A., Agrawal, S. K., and Murphey, R. 2002. Groups of unmanned vehicles: differential flatness, trajectory planning, and control . Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), Washington, DC, May, pp. 3461-3466 .
59.
Reif, J. and Sharir, M. 1994. Motion planning in the presence of moving obstacles . Journal of the ACM41(4): 764-790 .
60.
Reif, J. and Wang, H. 1997. Non-uniform discretization approximations for kinodynamic motion planning and its applications. Algorithms for Robotic Motion and Manipulation, J.-P. Laumond and M. Overmars, editors, A. K. Peters, Wellesley, MA , pp. 97-112.
61.
Richards, A., How, J., Schouwenaars, T., and Feron, E. 2001. Plume avoidance maneuver planning using mixed integer linear programming . Proceedings of the AIAA Guidance, Navigation and Control Conference, Montreal, Canada, August.
62.
Rizzi, A. A., Gowdy, J., and Hollis, R. L. 2001. Distributed coordination in modular precision assembly systems . International Journal of Robotics Research20(10): 819-838 .
63.
Sahar, G. and Hollerbach, J. M. 1986. Planning of minimum-time trajectories for robot arms . International Journal of Robotics Research5(3): 97-140 .
64.
Sanchez, G. and Latombe, J. 2002. On delaying collision checking in PRM planning—application to multi-robot coordination . International Journal of Robotics Research21(1): 5-26 .
65.
Scheuer, A. and Fraichard, T. 1997. Continuous-curvature path planning for car-like vehicles . Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Grenoble, France, September, Vol. 2, pp. 997-1003 .
66.
Schouwenaars, T., De Moor, B., Feron, E., and How, J. 2001. Mixed integer programming for multi-vehicle path planning . European Control Conference 2001, Porto, Portugal.
67.
Shiller, Z. and Chen, J. C. 1990. Optimal motion planning of autonomous vehicles in three-dimensional terrains . Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), Cincinnati, OH, May, pp. 198-203 .
68.
Shiller, Z. and Dubowsky, S. 1991. On computing the global time-optimal motion of robotic manipulators in the presence of obstacles . IEEE Transactions on Robotics and Automation7(6): 785-797 .
69.
Shiller, Z. and Lu, H. H. 1992. Computation of path constrained time-optimal motions with dynamic singularities . ASME Journal of Dynamic Systems, Measurement, and Control114: 34-40 .
70.
Shiller, Z., Large, F., and Sekhavat, S. 2001. Motion planning in dynamic environments: Obstacles moving along arbitrary trajectories . Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), Seoul, Korea, May, pp. 3716-3721 .
71.
Shin, Y. and Bien, Z. 1989. Collision-free trajectory planning for two robot arms . Robotica7: 205-212 .
72.
Shin, K. G. and McKay, N. D. 1985. Minimum-time control of robotic manipulators with geometric path constraints . IEEE Transactions on Automatic Control30(6): 531-541 .
73.
Shin, K. G. and Zheng, Q. 1992. Minimum-time collision-free trajectory planning for dual robot systems . IEEE Transactions on Robotics and Automation8(5): 641-644 .
74.
Simeon, T., Leroy, S., and Laumond, J.-P. 2002. Path coordination for multiple mobile robots: a resolution-complete algorithm . IEEE Transactions on Robotics and Automation18(1): 42-49 .
75.
Slotine, J.-J. and Yang, H. S. 1989. Improving the efficiency of time-optimal path-following algorithms . IEEE Transactions on Robotics and Automation5: 118-124 .
76.
Švestka, P. and Overmars, M. 1998. Coordinated path planning for multiple robots . Robotics and Autonomous Systems23(3): 125-152 .
77.
Tomlin, C., Pappas, G. J., and Sastry, S. 1998. Conflict resolution for air traffic management: a study in multi-agent hybrid systems . IEEE Transactions on Automatic Control43(4): 509-521 .
78.
von Stryk, O. 2000. User’s Guide for DIRCOL (Version 2.1): a direct collocation method for the numerical solution of optimal control problems. Technische Universitat Darmstadt .
79.
Zefran, M., Desai, J. P., and Kumar, V. 1997. Continuous motion plans for robotic systems with changing dynamic behavior. Algorithms for Robotic Motion and Manipulation, J.-P. Laumond and M. Overmars, editors, A. K. Peters, Wellesley, MA .
Supplementary Material
Please find the following supplemental material available below.
For Open Access articles published under a Creative Commons License, all supplemental material carries the same license as the article it is associated with.
For non-Open Access articles published, all supplemental material carries a non-exclusive license, and permission requests for re-use of supplemental material or any part of supplemental material shall be sent directly to the copyright owner as specified in the copyright notice associated with the article.
