Model-based Reactive Programming of Cooperative Vehicles for Mars Exploration

Model-based Reactive Programming of Cooperative Vehicles

for Mars Exploration

Brian C. Williams, Phil Kim, Michael Hofbaur, Jon How, John Kennell, Jason Loy,

Rob Ragno, John Stedl and Aisha Walcott

Artificial Intelligence and Space Systems Labs

Massachusetts Institute of Technology

Cooperative Mars Exploration

How do we coordinate heterogeneous teams of orbiters, rovers and air vehicles to perform globally optimal science exploration?

Cooperative Mars Testbed:

• 1 ATRV S.• 3 ATRV Jr • monster trucks• indoor blimps• Spheres formation flying vehicles

Cooperative Vehicles

High-level vehicle coordination(MERS)

Fast Agile Maneuvering(frazzoli, dahleh, feron)

Support: ONR & DARPA

Model-based Cooperative AutonomyPerform distributed, planning and execution by reasoning from

models of networked vehicles, their interactions and their environment:

• Program using a rich embedded language to describe concurrent team strategies.

• Plan and schedule coordinated activities of heterogenous vehicles to achieve science goals.– Distributed optimal planning– Combine activity planning, with global/ local kino-dynamic path

planning.

• Execute and monitor coordinated plans for individual subsystems.

• Isolate and diagnose individuals that lead to team failure. • Compensate for failures within the team through

reconfiguration, repair or replanning.

Outline

• The Reactive Model-based Programming Language (RMPL)

• Temporal Plan Networks (TPN)

• Activity Planning (Kirk)

• Path Planning

HOMEHOME

TWO

EnrouteCOLLECTION POINTCOLLECTION POINTRENDEZVOUSRENDEZVOUS

Diverge

SCIENCE AREA 1’SCIENCE AREA 1’

SCIENCE AREA 3SCIENCE AREA 3

Landing Site: ABC

Landing Site: XYZ

Properties: Team guided by a hierarchy of complex

strategies. Maneuvers are coordinated and time constrained. Novel events occur during critical phases. Quick response draws upon a library of

contingencies.

Example ScenarioONE

SCIENCE AREA 1SCIENCE AREA 1

Reactive Model-based Programming

Idea: Describe team behaviors by extending a rich reactive programming language (e.g., Esterel)

c If c next A Unless c next A A, B Always A

• Sensing/actuation activities• Conditional execution• Preemption• Full concurrency • Iteration

A [l,u] Choose {A, B}

• Timing• Contingency

Add temporal constraints and choice.

RMPL builds upon ideas fromTCC (gupta et al.)

Example Enroute Activity:

RendezvousRendezvous Rescue AreaRescue Area

Corridor 2

Corridor 1

Enroute

RMPL for Group-Enroute

Group-Enroute()[l,u] = { choose { do { Group-Traverse-Path(PATH1_1,PATH1_2,PATH1_3,RE_POS)

[l*90%,u*90%]; } maintaining PATH1_OK, do { Group-Traverse-Path(PATH2_1,PATH2_2,PATH2_3,RE_POS)

[l*90%,u*90%]; } maintaining PATH2_OK }; { Group-Transmit(OPS,ARRIVED)[0,2], do { Group-Wait(HOLD1,HOLD2)[0,u*10%] } watching PROCEED }}





Activities:





Conditionalityand Preemption:




[l*90%,u*90%]; } maintaining PATH2_OK } ; { Group-Transmit(OPS,ARRIVED)[0,2], do { Group-Wait(HOLD1,HOLD2)[0,u*10%] } watching PROCEED }}

Concurrency:


Group-Enroute()[l,u] = { choose { do { Group-Fly-Path(PATH1_1,PATH1_2,PATH1_3,RE_POS)

[l*90%,u*90%]; } maintaining PATH1_OK, do { Group-Fly-Path(PATH2_1,PATH2_2,PATH2_3,RE_POS)


Temporal Constraints:





Decision TheoreticChoice:

RMPL Interpreter

• select proceduresselect procedures• order actionsorder actions• allocate resourcesallocate resources

dynamically selects consistent alternatives, schedules events, monitors outcomes and plans contingencies.

Reactive Temporal Planner

Plan Runner

(Hidden) States

RMPL Program

CommandsObservables

Diagnosis &Mode Estimation

Reconfiguration& Repair

Model of NetworkedEmbedded

Vehicles

• monitor activitiesmonitor activities• diagnose plan diagnose plan

failuresfailures

• select execution select execution timestimes

• How do we provide fast, temporally flexible planning?

• Graph-based planners support efficient planning … but plans are totally order

How do we create temporally flexible plan graphs?

• Generalize simple temporal networks (temporal plan network TPN).

Kirk: Reactive Planner

Reactive Temporal Planner

RMPL Compiler

Temporal Plan Network (TPN)

Concurrent Plan

Selects schedulable trajectories of TPN

Expands to RRT to avoid obstacles

Generalizes Simple Temporal Networksto Choice

c If c next A Unless c next A A, B Always A

A [l,u] Choose {A, A’…}

Reactive Model-based Programming Language

Concurrent execution threads related by Simple Temporal Net

RRT-based Path Planner

Outline




• Path Planning

Enroute Activity:

1

4 5

8

9 10

13

2

11 12

Enroute

Group Fly Path Group Wait

Group Transmit

Activity (or sub-activity)

Science Target

• Start with flexible plan representation

Enroute Activity:

1

4 5

8

2Enroute [450,540]

[405, 486]

Group Traverse Group Wait

Group Transmit

[0, 54]

[0, 2]


Duration (temporal constraint)

[0, ]

[0, 0][0, 0]

[0, 0]

[0, 0]

[0, 0] [0, 0]

Science Target

• Start with flexible plan representation

Enroute Activity:

3

1

4 5

8

9 10

13

2

6 7 11 12

Enroute

Group Fly Path

Group Fly Path Group Wait

Group Transmit


Science Target

•TPN representation of Enroute activity and sub-activities

Enroute Activity:

3

1

4 5

8

2Enroute [450,540]

Group Traverse

[405, 486]

[405, 486]


Group Transmit

[0, 54]

[0, 2]



[0, ]

[0, 0]

[0, 0]

[0, 0]

[0, 0]

[0, 0]

[0, 0]

[0, 0]

[0, 0] [0, 0]

Science Target

• Add conditional nodes

Conditional node

Enroute Activity:

3

1

4 5

8

9 10

13

2

6 7 11 12

Enroute [450,540]

Group Traverse

[405, 486]

[405, 486]


Group Transmit

[0, 54]

[0, 2]



[0, ]

[0, 0]

[0, 0]

[0, 0]

[0, 0]

[0, 0]

[0, 0]

[0, 0]

[0, 0] [0, 0]

Ask( PATH1 = OK)

Ask( PATH2 = OK)

Ask( EXPLORE = OK)Science Target

•Add temporally extended, symbolic constraints

Symbolic constraint (Ask,Tell)

Conditional node

Outline




• Path Planning

Planning Group-Enroute

3

6

4 5[405,486]

Ask(PATH1=OK)

1 2

7

Ask(PATH2=OK)

8

[405,486]

[450,540]

Ask(PROCEED)

11

9 10

[0,54]

12

13

[0,2]

[0,]

To Plan:• Instantiate Group-Enroute

Group-Enroute

Group Traverse


Group Transmit

Science Target

Planning Group-Enroute

3

6

4 5[405,486]

Ask(PATH1=OK)

1 2

7

Ask(PATH2=OK)

8

[405,486]

[450,540]

Ask(PROCEED)

11

9 10

[0,54]

12

13

[0,2]

[0,]

[0,] [0,]

14 15

Tell(PATH1=OK)

[450,450]16 17

Tell(PROCEED)

[200,200]

s e[500,800]

[10,10] [0,]

To Plan:• Instantiate Group-Enroute• Add External Constraints

Group-Enroute

Group Traverse


Group Transmit

Science Target

Generates Schedulable Plan

3

6

4 5[405,486]

Ask(PATH1=OK)

1 2

7

Ask(PATH2=OK)

8

[405,486]

[450,540]

Ask(PROCEED)

11

9 10[0,54]

12

13

[0,2]

[0,]

14 15

Tell(PATH1=OK)

[450,450]16 17

Tell(PROCEED)

[200,200]

s e[500,800]

[10,10] [0,]

[0,] [0,]

Group-Enroute

Group Traverse


Group Transmit

Science Target

Trace optimal consistent trajectories• Check Schedulability • Satisfy and Protect Asks

To Plan:• Instantiate Group-Enroute• Add External Constraints

Planning Example

1 2

3 4 5 6

7 8 9

10 11 12

13 14

• Find paths from start-node to end-node

Start End

15 16 17 18

Planning Example

1 2

3 4 5 6

7 8 9

10 11 12

13 14

• Not a decision-node: Follow all outarcs

Start End

15 16 17 18

Planning Example

1 2

3 4 5 6

7 8 9

10 11 12

13 14


Start End

15 16 17 18

Planning Example

1 2

3 4 5 6

7 8 9

10 11 12

13 14


Start End

15 16 17 18

Planning Example

1 2

3 4 5 6

7 8 9

10 11 12

13 14

• Decision-node: Select a single outarc

Start End

15 16 17 18

Planning Example

1 2

3 4 5 6

7 8 9

10 11 12

13 14


Start End

15 16 17 18

Planning Example

1 2

3 4 5 6

7 8 9

10 11 12

13 14

• Continue

Start End

15 16 17 18

Planning Example

1 2

3 4 5 6

7 8 9

10 11 12

13 14


Start End

15 16 17 18

Planning Example

1 2

3 4 5 6

7 8 9

10 11 12

13 14

• Continue

Start End

15 16 17 18

Planning Example

1 2

3 4 5 6

7 8 9

10 11 12

13 14

Start End

15 16 17 18

Temporal Constraint Consistency

1 2

3 4 5 6

7 8 9

10 11 12

13 14

• When a path induces a cycle, check for negative cycle in the STN distance graph

15 16 17 18

[18,20]

[0,0]

[0,0]

[0,0] [0,0]

[0,0]

[0,0][0,0]

[0,0]

[0,0]

[0,]

[2,3]

[15,16]

[4,6]

[5,5][3,8]


1 2

3 4 5 6

7 8 9

10 11 12

13 14

• Example: Inconsistent

15 16 17 18

[18,20]

[0,0]

[0,0]

[0,0] [0,0]

[0,0]

[0,0][0,0]

[0,0]

[0,0]

[0,]

[2,3]

[15,16]

[4,6]

[5,5][3,8]


1 2

3 4 5 6

7 8 9

10 11 12

13 14

• Backtrack

15 16 17 18

[18,20]

[0,0]

[0,0]

[0,0] [0,0]

[0,0]

[0,0][0,0]

[0,0]

[0,0]

[0,]

[2,3]

[15,16]

[4,6]

[5,5][3,8]

[0,0]

[0,0] [12,13]

[0,0]


1 2

3 4 5 6

7 8 9

10 11 12

13 14

• Complete paths

15 16 17 18

[18,20]

[0,0]

[0,0]

[0,0] [0,0]

[0,0]

[0,0][0,0]

[0,0]

[0,0]

[0,]

[2,3]

[15,16]

[4,6]

[5,5][3,8]

[0,0]

[0,0] [12,13]

[0,0]

Symbolic Constraint Consistency

• Link and protect asks

• Avoid inconsistent telk

5

7 8 9

10 11 12

6{4,6}

{4,6}

{4,6} {6,9}

{5,8} {7,11}

{7,10}

{8,11}

ask(c)

tell(c)


• Example: Inconsistent

5

7 8 9

10 11 12

6{4,6}

{4,6}

{4,6} {6,9}

{5,8} {7,11}

{7,10}

{8,11}

ask(c)

tell(c)


• Example: Impose ordering

5

7 8 9

10 11 12

6{4,6}

{4,6}

{4,6} {7,9}

{5,8} {7,9}

{7,10}

{8,11}

ask(c)

tell(c)

[0,0]

Outline




• Distributed Optimal Planning

• Path Planning

3

1

4 5

8

9 10

13

2

6 7 11 12

[450,540]

•Want Distributed, Optimal Planning extend shortest path auction algorithm by Bertsekas

Enroute Activity:

3

1

4 5

8

9 10

13

2

6 7 11 12

425

440

30

1

0

0

0

0 0

0

0

0

0

[450,540]

0

•Edge weights represent cost of activity execution

Enroute Activity:

3

1

4 5

8

9 10

13

2

6 7 11 12

425

440

30

1

0

0

0

0 0

0

0

0

0

[450,540]

price = 0

price = 0

price = 0

price = 0

price = 0

price = 0

price = 0 price = 0

price = 0 price = 0

price = 0

price = 0

Path P = 1

0price = 0

Start Node : 1End Node: 2

•Demonstration of extended auction algorithm

Enroute Activity:

3

1

4 5

8

9 10

13

2

6 7 11 12

425

440

30

1

0

0

0

0 0

0

0

0

0

[450,540]

Extend Path

price = 0

price = 0

price = 0

price = 0

price = 0

price = 0

price = 0

Path P = 1 3

price = 0

price = 0 price = 0

price = 0

price = 0

0price = 0

Enroute Activity:

•Extended auction algorithm

3

1

4 5

8

9 10

13

2

6 7 11 12

425

440

30

1

0

0

0

0 0

0

0

0

0

[450,540]

price = 0

price = 0

price = 0

price = 0

price = 0

price = 0

price = 0

Path P = 1 3 6

Extend Path

price = 0

price = 0 price = 0

price = 0

price = 0

0price = 0

Enroute Activity:


3

1

4 5

8

9 10

13

2

6 7 11 12

425

440

30

1

0

0

0

0 0

0

0

0

0

[450,540]

price = 0

price = 0

price = 440

price = 0

price = 0

price = 0

price = 0

Path P = 1 3

Contract Path

price = 0

price = 0 price = 0

price = 0

price = 0

0price = 0

Enroute Activity:


3

1

4 5

8

9 10

13

2

6 7 11 12

425

440

30

1

0

0

0

0 0

0

0

0

0

[450,540]

price = 0

price = 0

price = 440

price = 0

price = 0

price = 0

price = 0

Path P = 1 3 4

Extend Pathprice = 0

price = 0 price = 0

price = 0

price = 0

0price = 0

Enroute Activity:


3

1

4 5

8

9 10

13

2

6 7 11 12

425

440

30

1

0

0

0

0 0

0

0

0

0

[450,540]

price = 0

price = 425

price = 440

price = 0

price = 0

price = 0

price = 0

Path P = 1 3

Contract Pathprice = 0

price = 0 price = 0

price = 0

price = 0

0price = 0

Enroute Activity:


3

1

4 5

8

9 10

13

2

6 7 11 12

425

440

30

1

0

0

0 0

0 0

0

0

0

0

[450,540]

Contract Path

price = 0

price = 425

price = 440

price = 0

price = 0

price = 0

price = 0

Path P = 1

price = 0

price = 0 price = 0

price = 0

price = 0price = 425

Enroute Activity:


3

1

4 5

8

9 10

13

2

6 7 11 12

425

440

30

1

0

0

0 0

0 0

0

0

0

0

[450,540]

price = 425

price = 425

price = 440

price = 0

price = 0

price = 0

price = 0

Path P = 1

price = 0

price = 0 price = 0

price = 0

price = 0price = 425

Enroute Activity:


3

1

4 5

8

9 10

13

2

6 7 11 12

425

440

30

1

0

0

0

0 0

0

0

0

0

[450,540]

Extend Path

price = 425

price = 425

price = 440

price = 0

price = 0

price = 0

price = 0

Path P = 1 3

price = 0

price = 0 price = 0

price = 0

price = 0

0price = 425


Enroute Activity:

3

1

4 5

8

9 10

13

2

6 7 11 12

425

440

30

1

0

0

0

0 0

0

0

0

0

[450,540]

price = 425

price = 425

price = 440

price = 0

price = 0

price = 0

price = 0

Path P = 1 3 4

price = 0

price = 0 price = 0

price = 0

price = 0

0price = 425

Extend Path

Enroute Activity:


3

1

4 5

8

9 10

13

2

6 7 11 12

425

440

30

1

0

0

0

0 0

0

0

0

0

[450,540]

price = 425

price = 425

price = 440

price = 0

price = 0

price = 0

price = 0

Path P = 1 3 4 5

price = 0

price = 0 price = 0

price = 0

price = 0

0price = 425

Extend Path

Enroute Activity:


3

1

4 5

8

9 10

13

2

6 7 11 12

425

440

30

1

0

0

0

0 0

0

0

0

0

[450,540]

price = 425

price = 425

price = 440

price = 0

price = 0

price = 0

price = 0

Path P = 1 3 4 5 8

price = 0

price = 0 price = 0

price = 0

price = 0

0price = 425

Extend Path

Enroute Activity:


3

1

4 5

8

9 10

13

2

6 7 11 12

425

440

30

1

0

0

0

0 0

0

0

0

0

[450,540]

price = 425

price = 425

price = 440

price = 0

price = 0

price = 0

price = 0 price = 0

price = 0 price = 0

price = 0

price = 0

0price = 425

Path P = 1 3 4 5 8 AND Node

Enroute Activity:


3

1

4 5

8

9 10

13

2

6 7 11 12

425

440

30

1

0

0

0

0 0

0

0

0

0

[450,540]

price = 425

price = 425

price = 440

price = 0

price = 0

price = 0

price = 0 price = 0

price = 0 price = 0

price = 0

price = 0

0price = 425

Path P = 1 3 4 5 8 AND NodeRun Auction for each branch concurrently

Enroute Activity:


3

1

4 5

8

9 10

13

2

6 7 11 12

425

440

30

1

0

0

0

0 0

0

0

0

0

[450,540]

price = 425

price = 425

price = 440

price = 0

price = 0

price = 0

price = 0 price = 0

price = 0 price = 0

price = 0

price = 0

0price = 425

Extend Path

Path P = 1 3 4 5 8

9

11

Extend Path

Enroute Activity:


3

1

4 5

8

9 10

13

2

6 7 11 12

425

440

30

1

0

0

0

0 0

0

0

0

0

[450,540]

price = 425

price = 425

price = 440

price = 0

price = 0

price = 0

price = 30 price = 0

price = 1 price = 0

price = 0

price = 0

0price = 425

Path P = 1 3 4 5 8

9

11

Enroute Activity:


3

1

4 5

8

9 10

13

2

6 7 11 12

425

440

30

1

0

0

0

0 0

0

0

0

0

[450,540]

price = 425

price = 425

price = 440

price = 0

price = 0

price = 0


price = 1 price = 0

price = 0

price = 0

0price = 425

Path P = 1 3 4 5 8

9 10

11 12

Extend Path

Extend Path

Enroute Activity:


3

1

4 5

8

9 10

13

2

6 7 11 12

425

440

30

1

0

0

0

0 0

0

0

0

0

[450,540]

price = 425

price = 425

price = 440

price = 0

price = 0

price = 0


price = 1 price = 0

price = 0

price = 0

0price = 425

Path P = 1 3 4 5 8

9 10

11 12

13

Extend Path

Extend Path

Enroute Activity:


3

1

4 5

8

9 10

13

2

6 7 11 12

425

440

30

1

0

0

0

0 0

0

0

0

0

[450,540]

price = 425

price = 425

price = 440

price = 0

price = 0

price = 0


price = 1 price = 0

price = 0

price = 0

0price = 425

Path P = 1 3 4 5 8

9 10

11 12

13 2

Extend Path

Enroute Activity:


3

1

4 5

8

9 10

13

2

6 7 11 12

425

440

30

1

0

0

0

0 0

0

0

0

0

[450,540]

price = 425

price = 425

price = 440

price = 0

price = 0

price = 0


price = 1 price = 0

price = 0

price = 0

0price = 425

Path P = 1 3 4 5 8

9 10

11 12

13 2

Extend Path

Enroute Activity:

•Path generated by extended auction algorithm

Start Node : 1End Node: 2

Outline




• Randomized, Kino-dynamic Path Planning

Enroute Activity:

3

1

4 5

8

9 10

13

2

6 7 11 12

Enroute [450,540]

Group Traverse

[405, 486]

[405, 486]


Group Transmit

[0, 54]

[0, 2]

[0, ]

[0, 0]

[0, 0]

[0, 0]

[0, 0]

[0, 0]

[0, 0]

[0, 0]

[0, 0] [0, 0]

Ask( PATH1 = OK)

Ask( PATH2 = OK)

Ask( PROCEED)

Traverse to Science Target

Science Target

•Closer look at Group Traverse sub-activity

Group Traverse sub-activity:

3

4

8

6 7

[0, 0][0, 0]

Ask( PATH2 = OK)

Ask( PATH2 = OK)

5

•Traverse through way points to science target

Group Traverse [405, 486]

Group Traverse [405, 486]

[0, 0] [0, 0]

3

4

8

6 7

[0, 0][0, 0]

5

•Traverse to science target area (with obstacles)•One obstacle between nodes 4 and 5•Two Obstacles between nodes 6 and 7

[0, 0] [0, 0]

Group Traverse sub-activity:

ObstacleObstacleObstacleObstacle

ObstacleObstacleObstacleObstacle ObstacleObstacleObstacleObstacle

How do we optimally select activities and paths?

Current Research:

• Perform global path planning using Rapidly-exploring Random Trees (RRTs) (la Valle).

• Perform local kino-dynamic path planning along path segments using hybrid maneuver automata (Frazzoli, Dahleh, Feron).

• Search for globally optimal plan by unifying TPN & RRT graphs, and by searching best first using distributed auction.

RRT: Example

3

4

8

6 7

[0, 0] [0, 0]

5

[0, 0] [0, 0]

Path 1

Path 2

3

4

8

6 7

[0, 0] [0, 0]

5

[0, 0] [0, 0]

xinit xgoal

Assume rovers take Path 1:

Path 1

Path 2

RRT: Example

4 5

xinit

Path 1

xgoalXobs

RRT: Example

4 5

xinit

Path 1

xgoalXobs

RRT: Example

4 5

xinit

Path 1

xgoalXobs

RRT: Example

4 5

xinit

Path 1

xgoalXobs

RRT: Example

4 5

xinit

Path 1

xgoalXobs

RRT: Example

4 5

xinit

Path 1

xgoalXobs

RRT: Example

4 5

xinit

Path 1

xgoalXobs

Common Node

RRT: Example

4 5

xinit

Path 1

xgoalXobs

RRT: Example

4 5

xinit

Path 1

xgoalXobs

RRT: Example

4 5

xinit

Path 1

xgoalXobs

RRT: Example

Current Status

• Kirk demonstrated on cooperative scenario using UAV simulation.• Development on Multi-Rover testbed currently in progress.• Distributed hybrid activity/path planning in progress.

Model-based Cooperative Autonomy

Perform distributed, planning and execution by reasoning from models of networked vehicles, their interactions and their environment:

• Use a rich embedded language to describe complex concurrent team strategies.

• Plan and schedule coordinated activities of heterogenous vehicles to achieve science goals.– Fast planning by search temporal plan network– Find optimal plan using distributed auction algorithms– Optimal planning combines activity planning, global and

local kino-dynamic path planning.• Execute, monitor coordinated plans for individual

subsystems.• Diagnose and Repair team behaviors.

Documents

Model-based Reactive Programming of Cooperative Vehicles for Mars Exploration