implan: scalable incremental motion planning for … · implan: scalable incremental motion...

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 ξ


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


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


Motion Plan Synthesis for a Swarm of Robots

Implan(Incremental Motion PLANner)

Specification: (¬collision) U reach


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


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


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



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



What if a priority assignment does not exist?




R2 R2'

R1

R1'

R2 R2'R1 R1'

prio(1) < prio(2) ∧ prio(1) > prio(2)




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



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



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


Properties of Implan

Generated trajectories are cost optimal, but may not betime optimal

The worst case computational complexity of Implan is thesame as Complan


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


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


Future Work

To Extend Implan to arbitrary LTL/MTL properties


Thank You!!http://www.seas.upenn.edu/∼isaha


implan: scalable incremental motion planning for … · implan: scalable incremental motion...

Documents