Upload
ngohanh
View
223
Download
0
Embed Size (px)
Citation preview
Implan: Scalable Incremental MotionPlanning for Multi-Robot Systems
Indranil SahaUC Berkeley and UPenn
Joint work with
Rattanachai Ramaithitima (UPenn), Vijay Kumar (UPenn),George Pappas (UPenn) and Sanjit Seshia (UC Berkeley)
Annual Meeting 2015
ExCAPE Annual Meeting 2015 Indranil Saha Incremental Motion Planning for Multi-Robot Systems 1/15
Motion Planning Problem
An input problem instance P = 〈R, I,PRIM,Workspace, ξ〉
R - The set of robots
I - Initial state of the group of robots
PRIM = [PRIM1,PRIM2, . . . ,PRIM|R|]
Workspace - Workspace dimension, position of obstacles
ξ - Specification given in Linear Temporal Logic
Definition (Motion Planning Problem)
Given an input problem P with specification ξ, synthesize atrajectory that satisfies ξ
ExCAPE Annual Meeting 2015 Indranil Saha Incremental Motion Planning for Multi-Robot Systems 2/15
Trajectory Synthesis via SMT Solving [IROS 2014]
Complan(COmpositional Motion PLANner)
http://www.seas.upenn.edu/∼isaha/complan.shtml
Φ(0)Prim1−−−→ Φ(1)
Prim2−−−→ Φ(2) . . .Φ(L− 1)PrimL−−−→ Φ(L)
Constraints: (Φ(0) ∈ I) ∧ ‖Transition‖ ∧ ‖Specification‖
Boolean combination of constraints from Linear Arithmetic andEquality with Uninterpreted Functions theories
Complan solves for the L motion primitives using an SMT solver
ExCAPE Annual Meeting 2015 Indranil Saha Incremental Motion Planning for Multi-Robot Systems 3/15
Limitation of Complan
The motion planning problem is reduced to a monolithic SMTsolving problem
The number of constraints for collision avoidance increasesquadratically with the number of robots
Complan has limited scalability
ExCAPE Annual Meeting 2015 Indranil Saha Incremental Motion Planning for Multi-Robot Systems 4/15
Motion Plan Synthesis for a Swarm of Robots
Implan(Incremental Motion PLANner)
Specification: (¬collision) U reach
ExCAPE Annual Meeting 2015 Indranil Saha Incremental Motion Planning for Multi-Robot Systems 5/15
Outline of the Algorithm
R2 R2'
R1
R1'
R2 R2'R1 R1'
Synthesize optimal trajectory for each robot independently
Assign Priority to find a feasible ordering for the robots
Synthesize final trajectories according to the assignedpriorities
Treat the robots with higher priorities as dynamic obstaclesIntroduce minimum delay to execute the optional trajectory toavoid collision
ExCAPE Annual Meeting 2015 Indranil Saha Incremental Motion Planning for Multi-Robot Systems 6/15
Priority Assignment Algorithm
InitObs(j) : The set of robots whose initial locations obstruct theoptimal trajectory of Robot Rj
FinalObs(j) : The set of robots whose final locations obstructthe optimal trajectory of Robot Rj
Algorithm:
1. Generate the following constraints:
∀Rj ∈ R,∀Rk ∈ R\{Rj} :Rk ∈ InitObs(j)→ prio(k) > prio(j)
∀Rj ∈ R,∀Rk ∈ R\{Rj} :Rk ∈ FinalObs(j)→ prio(j) > prio(k)
2. Solve the constraints using an SMT solver
ExCAPE Annual Meeting 2015 Indranil Saha Incremental Motion Planning for Multi-Robot Systems 7/15
Example1: Motion Plan Synthesis for 6 Robots
x
y
0 1 2 3 4 5 6 7 8 9 10
1
2
3
4
5
6
7
8
10
11
12
13
14
9
11 12 13 14
0
15
15 i1 i2
i3
i4
i5 i6
f1
f2
f3f4
f5
f4
f6
Ordering:1: R3 2: R6 3: R24: R4 5: R1 6: R5
ExCAPE Annual Meeting 2015 Indranil Saha Incremental Motion Planning for Multi-Robot Systems 8/15
Example1: Motion Plan Synthesis for 6 Robots
x
y
0 1 2 3 4 5 6 7 8 9 10
1
2
3
4
5
6
7
8
10
11
12
13
14
9
11 12 13 14
0
15
15 i1 i2
i3
i4
i5 i6
f1
f2
f3f4
f5
f4
f6
Ordering:1: R3 2: R6 3: R24: R4 5: R1 6: R5
Computation Time:
Complan: 27m40s
Implan: 7s
ExCAPE Annual Meeting 2015 Indranil Saha Incremental Motion Planning for Multi-Robot Systems 8/15
Priority Assignment Algorithm
What if a priority assignment does not exist?
ExCAPE Annual Meeting 2015 Indranil Saha Incremental Motion Planning for Multi-Robot Systems 9/15
Priority Assignment Algorithm
What if a priority assignment does not exist?
R2 R2'
R1
R1'
R2 R2'R1 R1'
prio(1) < prio(2) ∧ prio(1) > prio(2)
ExCAPE Annual Meeting 2015 Indranil Saha Incremental Motion Planning for Multi-Robot Systems 9/15
Priority Assignment Algorithm
What if a priority assignment does not exist?
R2 R2'
R1
R1'
R2 R2'R1 R1'
prio(1) < prio(2) ∧ prio(1) > prio(2)
Dependent robots are placed into a group
Groups are found by unsatisfiable cores generated by theSMT solver
ExCAPE Annual Meeting 2015 Indranil Saha Incremental Motion Planning for Multi-Robot Systems 9/15
Example2: Motion Plan Synthesis for 6 Robots
x
y
0 1 2 3 4 5 6 7 8 9 10
1
2
3
4
5
6
7
8
10
11
12
13
14
9
11 12 13 14
0
15
15 i1 i2
i3
i4
i5 i6
f1
f2
f3f4
f5
f4
f6
Ordering:1: R22: R4, R5, R63. R1, R3
ExCAPE Annual Meeting 2015 Indranil Saha Incremental Motion Planning for Multi-Robot Systems 10/15
Example2: Motion Plan Synthesis for 6 Robots
x
y
0 1 2 3 4 5 6 7 8 9 10
1
2
3
4
5
6
7
8
10
11
12
13
14
9
11 12 13 14
0
15
15 i1 i2
i3
i4
i5 i6
f1
f2
f3f4
f5
f4
f6
Ordering:1: R22: R4, R5, R63. R1, R3
Computation Time:
Complan: 3hr13m
Implan: 27m
ExCAPE Annual Meeting 2015 Indranil Saha Incremental Motion Planning for Multi-Robot Systems 10/15
Properties of Implan
Generated trajectories are cost optimal, but may not betime optimal
The worst case computational complexity of Implan is thesame as Complan
ExCAPE Annual Meeting 2015 Indranil Saha Incremental Motion Planning for Multi-Robot Systems 11/15
Example 3: Motion Planning in a Compact Workspace
Figure: caption
25 quadrotors movingin a closed place
Specification:¬collision U reach
Workspace Size:15.2m × 16.8m
Computation Time:4hr12m
ExCAPE Annual Meeting 2015 Indranil Saha Incremental Motion Planning for Multi-Robot Systems 12/15
Example 4: Incremental Motion Planning in a FreeWorkspace
Figure: caption
50 quadrotors movingin a free place
Specification:¬collision U reach
Workspace Size:15.2m × 16.8m
Computation Time:30m19s
ExCAPE Annual Meeting 2015 Indranil Saha Incremental Motion Planning for Multi-Robot Systems 13/15
Future Work
To Extend Implan to arbitrary LTL/MTL properties
ExCAPE Annual Meeting 2015 Indranil Saha Incremental Motion Planning for Multi-Robot Systems 14/15
Thank You!!http://www.seas.upenn.edu/∼isaha
ExCAPE Annual Meeting 2015 Indranil Saha Incremental Motion Planning for Multi-Robot Systems 15/15