Task Assignment and Trajectory Planning in Dynamic environments for Multiple Vehicles

http://www.diva-portal.org

This is the published version of a paper presented at RSS 2016 Workshop on Task and Motion Planning, Ann Arbor, Michigan, USA, June 19, 2016.

Citation for the original published paper:

David, J., Valencia, R., Iagnemma, K. (2016)

Task Assignment and Trajectory Planning in Dynamic environments for Multiple Vehicles.

In:

N.B. When citing this work, cite the original published paper.

Permanent link to this version:

http://urn.kb.se/resolve?urn=urn:nbn:se:hh:diva-31738

Task Assignment and Trajectory Planning in Dynamic environments for Multiple Vehicles

Jennifer David and Rafael Valencia School of Information and Technology

Halmstad University Halmstad, Sweden 30118

Karl Iagnemma Robotics Mobility Group, Massachusetts Institute of Technology,

MA, USA

Abstract— We consider the problem of finding collision-free trajectories for a fleet of automated guided vehicles (AGVs) working in ship ports and freight terminals. Our solution computes collision-free trajectories for a fleet of AGVs to pick up one or more containers and transport it to a given goal without colliding with other AGVs and obstacles. We propose an integrated framework for solving the goal assignment and trajectory planning problem minimizing the maximum cost over all vehicle trajectories using the classical Hungarian algorithm.

To deal with the dynamics in the environment, we refine our final trajectories with CHOMP (Covariant Hamiltonian optimization for motion planning) in order to trade off between path smoothness and dynamic obstacle avoidance.

Keywords: Multi-robot, task assignment, path planner

I. I

There are many ways to address the problem of motion planning for multiple robots. There are generally two ap- proaches for solving this issue - centralized and decentral- ized. In centralized approach, the configuration spaces of the individual robots are combined into one composite configu- ration space which is then searched for a path for the whole composite system. In contrast, the decoupled/decentralized approach computes separate paths for the individual robots and then resolves possible conflicts of the generated paths.

Though the former approach produces a complete and opti- mal solution, the time complexity is very huge. However, in case of the other, the solution is far from optimality.

For our intended application, our approach falls in between the two categories which is a decentralized system that is complete and guarantees optimality. The inspiration of this approach comes from the recent researches in multi robot coordination and task allocation methods which have adapted methods from operational research community. Here, the methods used for task assignment for a group of workers to goals has been adapted to multi-robot task allocation and trajectory planning problems as in [1]. By finding an optimal path and modifying it to be collision-free reduces the complexity for multi vehicle planning. Hence, coupling goal assignment with trajectory planning is more advantageous compared to the traditional two approaches in multi-vehicle path planning. The computational complexity of planning collision-free trajectories for multiple robots typically grows exponentially with the number of robots. This approach has recently been adapted by many researchers as in [1], [8].

Fig. 1. Front and Rear Steered AGV

II. SPADES F

Fig. 2 shows our planning framework, which we refer to as SPADES in this paper, short for Simultaneous Planning and Assignment in Dynamic EnvironmentS. Our framework consists of three main parts: task assignment, global path planning and trajectory planner. Our approach takes as input a set of goals that should be allocated to a set of available vehicles – e.g. in a container terminal, these goals are the locations where containers should be picked up and taken by the available vehicles. Then, an optimal assignment planner assigns vehicles to the nearest goals such that a overall total path cost is minimum. A global planner plans obstacle-free paths for each of the vehicles. The resulting trajectories are later continuously refined by a trajectory planner to avoid collisions with dynamic obstacles or static obstacles as well as to check for conflicts with other paths, treating all other vehicles as moving obstacles. Algorithm 1 shows our integrated planning framework with the three steps.

In the rest of this section we describe each step of our planning framework in detail.

A. Assignment

The Hungarian Algorithm [9] is adopted as our task as-

signment method in this algorithm. This approach solves the

linear assignment problem with computational complexity

bounded polynomially for N vehicles, O(N

), and is the

most efficient known method to optimally solve the linear

task assignment problem. The path costs to assignment

Fig. 2. SPADES Planning Framework.

Algorithm 1: SPADES Planning Framework.

Data: global map, local maps, localization data Result: list of obstacle-free trajectory points

1

Run Simultaneous Planning and Assignment

2

while T = 1 : N do

3

for each vehicle v

do

4

Run CHOMP (v

path, localMap

)

5

Execute trajectories from T − 1 to T

6

end

7

end

is computed as the path length between the shortest path between the starting locations of the AGVs, the pick-up containers and their respective goals. This path length is computed with the help of E* path planner algorithm and is not just the Euclidean distance. To this end, we compute a cost matrix of the entire site map which contains the path lengths in the matrices between the containers and the vehicles. This cost matrix is then fed into the assignment module which calculates the optimal assignment using the Hungarian algorithm as it is detailed in Algorithm 2.

B. Path Planner

The E-star algorithm [2] serves two purpose in this algo- rithm. Firstly, it is used along with the Hungarian algorithm to generate optimal task assignment. The high prioritized vehicles are given a higher penalty leading which is added to the path cost of the vehicle. Thus the Hungarian matrix captures both the priority of the vehicles and the path length costs as well. Secondly, the optimally assigned path is then used as the global path for the vehicle. By this way, the path planner is integrated into the task assignment module. Estar is a global path planner which supports dynamic replanning and path cost interpolation, resulting in lightweight repairing of plans and smooth paths during execution. The algorithm computes C-space samples of a crossing-time map defined

Algorithm 2: Simultaneous Planning and Assignment Data: Input site map with localization data

Result: Optimal task assignment

1

Calculate list of to-schedule containers c

and its goals g

Calculate list of available vehicles v

2

for each c

in containers do

3

for all vehicles in availistvehicles av

do

4

Compute E* path from c

to all av

5

return dist[A

]=dist[c

] to all vehicles

6

end

7

end

8

for each c

in containers do

9

Compute E star distance transform from c

to g

10

return dist(B

)=dist(c

) to its g

11

end

12

for each c

in containers do

13

for each vehicle in av

do

14

total path cost of c

= dist(A

) + dist (B

)

15

return cost matrix

16

end

17

end

18

run task assignment algorithm

19

return optimal task assignment

by the monotonic expansion of a continuous closed surface or a contour from the goal through the environment By modulating the propagation speed in function of environment characteristics, i.e. the effort of traversing certain regions, the crossing-time map becomes a navigation function that reflects the influence of the environment on the optimal-path of the robot, and it suffices to follow its steepest gradient from any point to drive the vehicle to the goal. Thus, E*

is employed to provide assignment costs in the form of the path lengths computed with this approach to our optimal task assignment algorithm and also act as a global planner.

C. Trajectory Refinement

CHOMP [4] (Covariant hamiltonian optimization for mo- tion planning) is a method for trajectory optimization invari- ant to re-parametrization that iteratively improves the quality of an initial trajectory, optimizing an objective functional.

The goal of CHOMP is to iteratively improve the quality of an initial trajectory by optimizing a functional that trades off between a smoothness and an obstacle avoidance component.

Thus, this approach is used as a local planner for avoiding dynamic obstacles. Our second main contribution in this paper is an extension to the original formulation of CHOMP to deal with AGVs considering its curvature constraints as well as collision avoidance. We also extend this method to resolve for path conflicts between multiple vehicles in the vicinity by iterating the planner over the individual obstacle components.

III. D

The task assignment module is treated as a SingleRobot-

SingleTask-Instantaneous assignment method [10]. Hence,

Fig. 3. Simulated Harbour Scenario

by the linear assignment of Hungarian algorithm, one vehicle will be assigned to one container and one container will be assigned to one vehicle such that the total path cost is minimum. Hence, with algorithm will not work with dynamic assignments or assignment of multiple containers to a single vehicle. Future work includes replacing the task assignment with a task scheduler that can handle dynamic assignments with complicated time-schedules. The assigned path plan which is when given to CHOMP, may sometime require complete replanning of the path by the global planner. In that case, the task assignment solutions may need not be optimal.

IV. C

In this paper we introduced SPADES, an integrated frame- work for solving the goal assignment and trajectory planning simultaneously, suited for dynamic environments. This inte- grated framework for combined task and motion planning helps in dealing the problem of multi-vehicle planning effec- tively. The high level task-assignment planner seeks to find an optimal assignment plan with spatial constraints computed from the path planner.

A

The authors would like to thank Roland Philippsen for his valuable guidance and contributions in this work. This work has been supported by the EU Project CargoAnts FP7- 605598.

R

[1] Matthew Turpin, Karthik Mohta, Nathan Michael, Vijaya Kumar. Goal Assignment and Trajectory Planning for Large Teams of Aerial Robots Autonomous RobotsDec 2014.

[2] Roland Philippsen and Roland Siegwart. Smooth and efficient obstacle avoidance for a tour guide robot. In Proc. IEEE Int. Conf. Robotics and Automation (ICRA), 2003.

[3] Sean Quinlan and Oussama Khatib. Elastic bands: connecting path planning and control.In Proc. IEEE Int. Conf. Robotics and Automa- tion (ICRA), 1993.

[4] Nathan Ratliff, Matt Zucker, J. Andrew Bagnell, and Siddhartha Srinivasa. CHOMP: Gradient optimization techniques for efficient motion planning. In Proc. Int. Conf. Robotics and Automation (ICRA), 2009.

[5] Jur van den Berg, Stephen J. Guy, Ming Lin, and Dinesh Manocha.

Reciprocal n-body collision avoidance. Springer Tracts in Advanced Robotics, Robotics Research: The 14th International Symposium (ISRR), 70, 2011.

[6] Lantao Liu, Dylan A. Shell, Nathan Michael. From Selfish Auctioning to Incentivized Marketing Autonomous Robots Dec 2014.

[7] Lantao Liu, Dylan A. Shell, An anytime assignment algorithm: From local task swapping to global optimality.Autonomous Robots 35: 271- 286, 2013.

[8] Jingjin Yu and M LaValle, Distance optimal formation control on graphs with a tight convergence time guarantee. In Decision and Control (CDC), 2012 IEEE 51st Annual Conference on, pages 4023- 4028, 2012.

[9] Kuhn, Harold W. The hungarian method for the assignment prob- lem.50 Years of Integer Programming 1958-2008, Springer Berlin Heidelberg, 2010. 29-47.

[10] Gerkey, Brian P., and Maja J. Matari. A formal analysis and taxonomy of task allocation in multi-robot systems. The International Journal of Robotics Research23.9 (2004): 939-954.