Integrating Vehicle Routing and Motion Planning

Size: px

Start display at page:

Download "Integrating Vehicle Routing and Motion Planning"

Jonathan Glenn
8 years ago
Views:

1 Integrating Vehicle Routing and Motion Planning Scott Kiesel, Ethan Burns, Christopher Wilt and Wheeler Ruml Department of Computer Science We are grateful for funding from the DARPA CSSG program (grant HR ) and NSF (grant IIS ). Scott Kiesel (UNH) Integrating Vehicle Routing and Motion Planning 1 / 24

for funding from the DARPA CSSG program (grant HR0011-09-1-0021) and NSF (grant

2 Contributions Contributions WAMP An Instance Application Central Challenge Surrogate Objective 1. New problem: Waypoint Allocation and Motion Planning (a) WAMP combines task planning and motion planning (b) vehicle routing but now with real routing! 2. Efficient solver (a) integrates tabu search, blind search, heuristic search, linear programming and simple temporal networks 3. Meets application requirements (a) 2.5x faster and more scalable than industrial partner s Scott Kiesel (UNH) Integrating Vehicle Routing and Motion Planning 2 / 24

3 Combining Vehicle Routing and Motion Planning Contributions WAMP An Instance Application Central Challenge Surrogate Objective Vehicle Routing allocate tasks to vehicles routes given as a distance matrix objective: find cheapest ordering temporal constraints Scott Kiesel (UNH) Integrating Vehicle Routing and Motion Planning 3 / 24

to vehicles routes given as a distance matrix objective: find cheapest ordering

4 Combining Vehicle Routing and Motion Planning Contributions WAMP An Instance Application Central Challenge Surrogate Objective Vehicle Routing allocate tasks to vehicles routes given as a distance matrix objective: find cheapest ordering temporal constraints Motion Planning find feasible trajectory continuous space respect vehicle limitations obstacles objective: minimize time 0 Scott Kiesel (UNH) Integrating Vehicle Routing and Motion Planning 3 / 24

cheapest ordering temporal constraints Motion Planning find feasible trajectory continuous space respect vehicle

5 Combining Vehicle Routing and Motion Planning Contributions WAMP An Instance Application Central Challenge Surrogate Objective WAMP allocate tasks to vehicles routes given as a distance matrix objective: find cheapest ordering temporal constraints find feasible trajectory continuous space respect vehicle limitations obstacles objective: minimize time 4 7 varying traversal costs objective: minimize time and cost naturally combines task allocation and motion planning Scott Kiesel (UNH) Integrating Vehicle Routing and Motion Planning 3 / 24

trajectory continuous space respect vehicle limitations 0 1 2 6 5 3 obstacles objective: minimize time 4 7 varying traversal costs

6 A Realistic-Sized Instance Contributions WAMP An Instance Application Central Challenge Surrogate Objective Scott Kiesel (UNH) Integrating Vehicle Routing and Motion Planning 4 / 24

21 19 24 32 4 22 12 14 2833 16 30 23 36 37 5 1811 17 2 2529 1 10 8 13 20 7

7 Application: Battlespace Management Contributions WAMP An Instance Application Central Challenge Surrogate Objective A large instance: 16 aircraft varying velocities and turning radii 40 waypoints time windows relative temporal constraints 40 radar sensitive (cost) zones strict no-fly zones 7 second time limit Scott Kiesel (UNH) Integrating Vehicle Routing and Motion Planning 5 / 24

waypoints time windows relative temporal constraints 40 radar sensitive (cost) zones strict

8 Central Challenge Contributions WAMP An Instance Application Central Challenge Surrogate Objective 1. Can t do task allocation without routing costs need cost from one waypoint to next 2. Can t find routing costs without motion paths paths may intersect areas of high cost 3. Can t find motion paths without leg durations is there time to navigate around high cost? 4. Can t assign leg durations without time/cost tradeoff which legs benefit most from additional time? Scott Kiesel (UNH) Integrating Vehicle Routing and Motion Planning 6 / 24

Can t find routing costs without motion paths paths may intersect areas of high cost 3.

9 Surrogate Objective Contributions WAMP An Instance Application Central Challenge Surrogate Objective fastest route is too expensive cheapest route is too long Scott Kiesel (UNH) Integrating Vehicle Routing and Motion Planning 7 / 24

fastest route is too expensive cheapest route is too long 1 0

10 Overview Step 1 Step 2 Step 3 Step 4 Feedback Scott Kiesel (UNH) Integrating Vehicle Routing and Motion Planning 8 / 24

11 Overview Overview Step 1 Step 2 Step 3 Step 4 Feedback 1. Precomputation: find surrogate objective endpoints surrogate objective 2. Sequencer: assign and order waypoints to vehicles ordering 3. Linear Program: assign timepoints to waypoints timetable 4. Routing: find motion plans between waypoints 5. (Feedback: provide new information to previous layers) new constraints Scott Kiesel (UNH) Integrating Vehicle Routing and Motion Planning 9 / 24

Sequencer: assign and order waypoints to vehicles ordering 3.

12 Step 1: Precomputation Overview Step 1 Step 2 Step 3 Step 4 Feedback Input: problem Output: surrogate objective Techniques: Dijkstra Scott Kiesel (UNH) Integrating Vehicle Routing and Motion Planning 10 / 24

9 21 4 27 0 38 19 24 32 22 12 14 2833 16 30 23 36 37 5 1811 17 2 2529 1 10 8 13

Step 1: Precomputation Overview Step 1 Step 2 Step 3 Step 4 Feedback Input: problem Output: surrogate objective Techniques: Dijkstra 43 42 41 40 35 15 3 26 6 9 21

13 Step 1: Precomputation Overview Step 1 Step 2 Step 3 Step 4 Feedback Input: problem Output: surrogate objective Techniques: Dijkstra Scott Kiesel (UNH) Integrating Vehicle Routing and Motion Planning 10 / 24

14 Step 2: Sequencer Overview Step 1 Step 2 Step 3 Step 4 Feedback Input: surrogate objective Output: feasible ordering : {w 2,w 3,w 1 } Techniques: tabu search (based on Lau, Sim and Teo, 2003) simple temporal network (STN) Scott Kiesel (UNH) Integrating Vehicle Routing and Motion Planning 11 / 24

Techniques: tabu search (based on Lau, Sim and Teo, 2003) simple temporal

15 Step 3: Linear Program Overview Step 1 Step 2 Step 3 Step 4 Feedback Input: feasible ordering : {w 2,w 3,w 1 }, surrogate objective Output: timetable : {w 2 = 2,w 3 = 3,w 1 = 5} Techniques: linear programming Scott Kiesel (UNH) Integrating Vehicle Routing and Motion Planning 12 / 24

timetable : {w 2 = 2,w 3 = 3,w 1 = 5} Techniques: linear programming

16 Step 4: Router Overview Step 1 Step 2 Step 3 Step 4 Feedback Input: timetable : {w 2 = 2,w 3 = 3,w 1 = 5} Output: solution Techniques: discretized A* search location, time, smoothing discretized A* search temporal pruning: t(s,n)+ˆt(n,g) > TT(g) re-expansions: g(n) < g(n ) but t(n) > t(n ) resumable Scott Kiesel (UNH) Integrating Vehicle Routing and Motion Planning 13 / 24

discretized A* search temporal pruning: t(s,n)+ˆt(n,g) > TT(g) re-expansions: g(n) < g(n )

17 Step 4: Router Overview Step 1 Step 2 Step 3 Step 4 Feedback Input: timetable : {w 2 = 2,w 3 = 3,w 1 = 5} Output: solution Techniques: discretized A* search location, time, smoothing smoothing Scott Kiesel (UNH) Integrating Vehicle Routing and Motion Planning 13 / 24

Techniques: discretized A* search location, time, smoothing

18 Feedback Overview Step 1 Step 2 Step 3 Step 4 Feedback Router : leg can not be routed to achieve timetable new constraints Linear Program : LP can not be solved with new constraint new constraints Sequencer Scott Kiesel (UNH) Integrating Vehicle Routing and Motion Planning 14 / 24

: LP can not be solved with new constraint new constraints Sequencer

19 Small Instances Sequencer Router Realistic Scott Kiesel (UNH) Integrating Vehicle Routing and Motion Planning 15 / 24

20 Experiment: Small Instance Scaling Small Instances Sequencer Router Realistic Test scalability against a unified A* Search single vehicle x,y,θ,t no temporal constraints spanning tree heuristic infinite time w/bounded memory (7.5GB) # Waypoints Failure Rate 1 24% 2 64% 3 88% 4 98% 5 98% 6 100% Generic A* does not scale to meet our requirements Scott Kiesel (UNH) Integrating Vehicle Routing and Motion Planning 16 / 24

21 Experiment: Sequencer Stressing Small Instances Sequencer Router Realistic Test ability to find quality orderings single vehicle with ǫ turn radius no temporal constraints uniform cost and no keep-out zones TSP instances factor of optimal cost M G NN 40 tasks M G NN 100 tasks The sequencer produces near optimal waypoint orderings for TSP instances Scott Kiesel (UNH) Integrating Vehicle Routing and Motion Planning 17 / 24

22 Experiment: Router Stressing Small Instances Sequencer Router Realistic Test ability to find solution paths that minimize cost single vehicle no temporal constraints The router produces high quality low level plans for complex cost instances Scott Kiesel (UNH) Integrating Vehicle Routing and Motion Planning 18 / 24

23 Experiment: Realistic Instances Test effects of scaling the number of waypoints 4 vehicles Small Instances Sequencer Router Realistic Scaling waypoints final sol cost min greedy LP log10 total wall time The system scales with an increasing number of waypoints Scott Kiesel (UNH) Integrating Vehicle Routing and Motion Planning 19 / 24

24 Experiment: Realistic Instances Test effects of scaling the number of vehicles 20 waypoints Small Instances Sequencer Router Realistic final sol cost Scaling vehicles min greedy LP log10 total wall time The system scales with an increasing number of vehicles Scott Kiesel (UNH) Integrating Vehicle Routing and Motion Planning 19 / 24

25 Summary Advertising Scott Kiesel (UNH) Integrating Vehicle Routing and Motion Planning 20 / 24

26 Summary Summary Advertising 1. New problem: Waypoint Allocation and Motion Planning (a) combines task planning and motion planning (b) vehicle routing but now with real routing 2. Efficient Solver (a) integrates tabu search, blind search, heuristic search, linear programming and simple temporal networks 3. Meets Application Requirements (a) 2.5x faster and more scalable than industrial partner s Scott Kiesel (UNH) Integrating Vehicle Routing and Motion Planning 21 / 24

27 The University of New Hampshire Summary Advertising Tell your students to apply to grad school in CS at UNH! friendly faculty funding individual attention beautiful campus low cost of living easy access to Boston, White Mountains strong in AI, infoviz, networking, bioinformatics Scott Kiesel (UNH) Integrating Vehicle Routing and Motion Planning 22 / 24

28 Back-up Slides Formulation Back-up Slides Scott Kiesel (UNH) Integrating Vehicle Routing and Motion Planning 23 / 24

29 Problem Formulation WAMP : Size,V,W,K,C,R Back-up Slides Formulation V: Vehicles v i = x 0,y 0,θ 0,v,r W: Waypoints w i = x,y,r,θ 0,θ 1,t s,t e,a A V K: Keep-Out Zones k i = x 0,y 0,x 1,y 1,x 2,y 2 C: Cost Zones c i = x,y,h,σ x,σ y,c R: Relative Constraints r i = w i,w j,min,max Scott Kiesel (UNH) Integrating Vehicle Routing and Motion Planning 24 / 24

Integrating Vehicle Routing and Motion Planning

Integrating Vehicle Routing and Motion Planning Scott Kiesel and Ethan Burns and Christopher Wilt and Wheeler Ruml Department of Computer Science University of New Hampshire Durham, NH 03824 USA skiesel,