model-based programming of cooperating robots brian c. williams, jonathan kennell, i-hsiang shu, and...

Model-based Programming of Cooperating Robots

Brian C. Williams,

Jonathan Kennell, I-hsiang Shu, and Raj Krishnan

Artificial Intelligence and Space Systems Labs

Massachusetts Institute of Technology

in Collaboration with

Maria Fox and Jon How

Outline

• Model-based Programming of Mobile Vehicles– Example: Cooperative Mars Rovers– Kirk Model-based Executive for Mobile Robots

• Demo: UAV Squadron Simulation – direct control

• Coaching Heterogeneous Teams– Example: Forest Fire Rescue– Kirk with Generative Activity Planner

• Demo: UAV Squadron Simulation – indirect control– Integration with MILP Path Planning

• Demo: Forest Fire Rescue on ATRV trucks• Demo: UAV Testbed

Example:Programming Cooperating Rovers

OEP

Wireless sensor networks

Outline






Kirk Model-based Execution System Overview

Mission Controller

RMPL control program

StrategyMacro Library

Strategy macrodecomposition

schedulable plan

Mission Developer

Strategy Selection

TPN Planner

Visibility Graph

Example Enroute Activity:

RendezvousRendezvous Rescue AreaRescue Area

Corridor 2

Corridor 1

Enroute

RMPL for Group-Enroute

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)

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


Mission Controller



Strategy macrodecomposition

schedulable plan

Mission Developer

Strategy Selection

TPN Planner

Visibility Graph

3

1

4 5

8

9 10

13

2

6 7 11 12

Enroute [450,540]

Group Traverse

[405, 486]

[405, 486]

Group Traverse Group Wait

Group Transmit

[0, 54]

[0, 2]

Activity (or sub-activity)

Duration (temporal constraint)

[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

Symbolic constraint (Ask,Tell)

Conditional node

Enroute Activity Encoded as a Temporal Plan Network

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,]

Group-Enroute

Group Traverse


Group Transmit

Science Target

Instantiated Enroute Activity

•Add environmental constraints

Activity (or sub-activity)

Duration (temporal constraint)

Symbolic constraint (Ask,Tell)

Conditional node

External constraints

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

To Plan, . . . perform the following hierarchically:• Trace trajectories • Check schedulability

• Satisfy and protect asks

Improve Temporal Consistency Checking Algorithm by using Incremental Methods

• Observation:– Frequent temporal consistency checks allow for improved planning

speed by using a fast incremental temporal consistency algorithm, ITC.– Both when a plan breaks and during incremental candidate plan

generation, new candidate plans are similar in structure to previous candidate plans.

• Solution:– Reuse the work of previously computed candidate plans, and update

only those nodes that need to be updated. – Similar to common incremental algorithms such as Incremental A*,

Dynamic A*, and Truth Maintanence System (TMS).

• Evalution:– Data shows that using the ITC algorithm improves temporal

consistency checks by about an order of magnitude for test cases involving cooperative air vehicles.

Performance Improvements

Comparison of Temporal Consistency Checking Algorithm Runtime

Comparison of the Number of Queue Insertions for Temporal Consistency Checking Algorithms

Temporal Consistency Algorithm (April ’03)

ITC Algorithm (current)

Temporal Consistency Algorithm (April ’03)

ITC Algorithm (current)

Number of UAVs

Number of UAVs

Que

ue In

sert

ions

Alg

orith

m R

untim

e

• UAVs performs 5 total activities, in which 2 randomly selected targets are chosen.

Outline






Demo 1: Detailed Guidance

Mission Specification:

1. Start with 9 combat UAVs at Headquarters.

2. UAVs go to station to rearm.

3. Each UAV then concurrently:

a) Goes to its assigned target and attacks.

• If short-range SAMs active, attack last (risk is high),

• Else, attack SAMs first (before they become active).

b) Goes to station for refueling.

c) Confirms the success of its attack.

4. All UAVs then return to Headquarters.

15 Targets

4 long range SAM sites

circle indicates range

9 Combat UAV

RefuelStation

RearmStation

Blue ForceHeadquarters

Demonstrates:

• Complex coordination of activities.

• Consistent option selection

• Simultaneous roadmap path planning

• Dynamic scheduling

Outline






Example: Coaching Heterogeneous Teams

OEP•Search and Rescue•Ocean Exploration

A dozen vehicles is too many to micro manage→ Act as a coach:

• Specify evolution of state and location.

Forest Fire Rescue

• Goal: retrieve family from fire.

• Rescue cannot take place until the local fire is suppressed.

• Retrofit one rescue vehicle for fire suppression

Ambulance

Rescue Point

Fire

Fire Line

Forest

Game Plan

Ambulance

Rescue Point

Fire

Fire Line

Forest

Phase 1


• Rescue cannot take place until local fire suppressed.

• Retrofit one vehicle for fire suppression

Game Plan

Ambulance

Rescue Point

Fire

Fire Line

Forest

Phase 2


• Rescue cannot take place until local fire suppressed.

• Retrofit one vehicle for fire suppression

Outline







Mission Controller



Strategy macrodecompositionStrategy Selection

TPN Planner

state configuration goals

Operators,Tactics,

Scenario Model

environment and action data

Activity Planning

Generative Activity Planner

schedulable planwith rationale

• Strategy Selection determines the optimal rules / strategies to accomplish mission goals.

• Activity Planning figures out how to achieve mission goals within strategic framework using available low-level actions.

Mission Developer

Visibility Graph

Human / ComputerInterface

MILP Path-Planning

RMPL Control Program

• (defclass rescue-team (execute () (sequence (parallel [l1,u1] (tell-start(at uav1 Ambulance)) (tell-start(at uav2 Ambulance)) (ask-end(suppressed Fire)) ) (parallel [l2,u2] (tell-start(at family RescuePoint)) (ask-end(rescued family)) (ask-end(at uav1 Ambulance)) (ask-end(at uav2 Ambulance)) ) ) ))

Phase 1

Phase 2

Initial State

IntermediateState

Goal State

Environment Model

• Terrain Map

• Object instantiations:– UAV uav1– UAV uav2– RESCU-READY uav1– RESCUE-READY uav2– IN-DISTRESS family– LOCATION Ambulance– LOCATION Fire– LOCATION RescuePoint

Vehicle Specifications

• Vehicle linearized dynamics

• Vehicle primitive operators:

– Fly(V,A,B)• move UAV “V” from location “A” to location “B”

– Refit(V)• Prepare UAV “V” to drop fire retardant

– Drop(V,A)• Drop fire retardant at location “A” with UAV “V”

– Rescue(V,P,A)• Rescue people “P” in distress with UAV “V” at location “A”


Mission Controller




TPN Planner


Operators,Tactics,

Scenario Model





Mission Developer

Activity Planning


Visibility Graph


MILP Path-Planning

Kirk Constructs Vehicle Activity Plan Using a Generative Temporal Planner

Generative Temporal Planner

Mission Goal State Plan

Vehicle Activity Plan

Use Atomic Generative Planner (GraphPlan – Blum & Furst)

To Generate Operators and Precedence

Extract Temporal Planand Check Schedulability

Translate to Planning Problemwith Atomic Operators

Approach:• Encode Goal Plan using an LPGP-style encoding • Prototype using LPGP [Fox/Long, CP03]

Vehicle OperatorDefinitions

Kirk extracts a least commitment plan and generates a rationale

Generated Activity Plan

CP-Inv-1[0,100]

CP-Start

Fly-StartFire

CP-Inv-1[0,100]

Fly-Inv[20,+INF]

Refit-Start

Fly-End

Refit-Inv[10,+INF]

CP-Inv-1[0,100]

CP-Inv-1[0,100]

Suppress-Start

CP-Inv-1[0,100]

Suppress-Inv[10,20]

Refit-End Fly-Start

Suppress-End

CP-Inv-1[0,100]

CP-Inv-1[0,100]

Fly-Inv[20,+INF]

CP-Midpoint

Fly-Inv[20,+INF]

[0,100]

[20,+INF] [10,20]

Outline






Demo 2: Strategic Guidance

Mission Specification (as states):

1. available vehicles at Rearm, Refuel & HQ.

2. All three UAVs must be fully fueled.

3. The two targets must be destroyed and confirmed,and the UAVs back at HQ.

2 Targets

4 SAM site (obstacle)

circle indicates range

1 Combat UAV

2 Reconnaissance UAVRefuelStation

RearmStation

Blue ForceHeadquarters

Demonstrates:

• High-level state commanding

• Generative activity planning

• with Vehicle assignment

Outline







Mission Controller




TPN Planner


Operators,Tactics,

Scenario Model





Mission Developer

Activity Planning


Visibility Graph


MILP Path-Planning

-100 -50 0 50 100-100

-80

-60

-40

-20

0

20

40

60

80

100

Aircraft Avoidance Example

• 3 UAV’s cross paths

• Straight line paths produce collision

• “Roundabout” is the best maneuver– Vehicles pass at limits

of avoidance regions– Requires coordination

• Solution emerges from the optimization problem

Work by Jon How et al., MIT

Trajectory Planning withObstacle Avoidance

• Representation of obstacles (A) as variables in a CSP

• Selection of values a assign a value to each clause vi in V, and results in a feasible region that the vehicle can be (B)

• Other selections result in infeasibilities (C)

• Goal of trajectory planning is to develop a path across the terrain that avoids all obstacles by resolving all clauses in the CSP

Trajectory Planning withObstacle Avoidance

• Search through the space of possible resolutions to the obstacles will result in infeasibilities at certain nodes

• Search is guided by leveraging Conflict-Directed A*, which identifies a kernel of a conflict and uses it to skip over other infeasible states

• Fast execution is dependent on a way of identifying and using (extracting) conflicts quickly

Outline






Scenario

Tactic:

Red to suppress Fire before blue comes within its range

UAV(with Fire

Suppressant)

UAV(rescue)Family

Fire

Safe point

Mountain

Scenario

Small Fire

Both take direct route

Blue clear during Fire suppression

Mountain

Scenario

(continued)

Mountain

ScenarioLarger Fire radius

Still both on direct route

Blue in range before fire suppression

Mountain

Mountain

ScenarioLarger Fire radius

Blue goes via staging point

Blue clear when fire suppressed

Mountain

Scenario(continued)

Experiment

• SAM radius small• Fire radius large

Demonstration of combinedCapabilities

Outline






Trajectory Optimization

Plan Processor

Cloud Cap Interface

UnplannedEvents

UAV Testbed

Cloud Cap Transmitter

Avionics

Groundstation

Operator

Plan Decisions

Mission State

900 Mhz Datalink

• Autopilots perform waypoint control

• Planner interacts exactly as with trucks

Dynamic Responses to Environment

A

B

Summary – Cooperative Robot Scenarios

• Combined activity / path-planning– Should solve unified problem optimally– Solution: integration with visibility graph, RRT, MILP

• Intra-team dependencies– Achieving mission goals depends on coordinated control, meeting deadlines,

etc.– Solution: temporally-flexible TPN plan representation

• Heterogeneous teams– Need to allow different operators / dynamics for different types of vehicles– Solution: RMPL

• Overlapping vehicle capabilities– Need to solve assignment problem efficiently– Need to support contingencies in case of vehicle loss– Solution: fast planning algorithms, reactive planning with programmer-

specified contingencies

model-based programming of cooperating robots brian c. williams, jonathan kennell, i-hsiang shu, and...

Documents

uav testbedexample

mobile robotsdemo

group traverse405

milp path planningdemo

atrv trucksdemo

okask path2

temporal plan network3645405

rendezvousrescue areacorridor