Multi-vehicle Cooperative ControlRaffaello D’Andrea

Mechanical & Aerospace Engineering Cornell University

Progress on RoboFlag Test-bed MLD approach to Multi-Vehicle Cooperation Obstacle Avoidance in Dynamic Environments Path Planning with Uncertainty

OUTLINE

RoboFlag

An experimental platform for multi-vehicle cooperative control in

uncertain and adversarial environments

ACTUATE

SENSE

LOW LEVELCONTROL

HIGH LEVELCONTROL

COMMS

VEHICLE

SENSE

COMMS

PROCESSINGGLOBALSENSING

COMMS

HIGH LEVELDECISION MAKING

CENTRALCONTROL

COMMUNICATIONS NETWORKCOMMS

COMMS COMMS

SYSTEMS OF INTEREST

HUMANINTERFACE

COMMS

COMMS

What is RoboFlag?

RoboFlag System

..

Overhead camerasVision computer

Arbiter

Computersfor each entity

...

RF transceiver

Leverage RoboCup Technology

SOFTWARE ARCHITECTURE

VEHICLEHIGH LEVEL

CONTROL

LOW LEVEL CONTROL INTERFACE

COMMUNICATIONS NETWORKSIMULATOR

WIRELESSINTERFACE

CENTRALCONTROL

MACHINE VISIONBASED

GLOBAL ANDLOCAL SENSING

VEHICLEHIGH LEVEL

CONTROL

ARBITER

VEHICLELOW LEVEL

CONTROL

VEHICLELOW LEVEL

CONTROL

LOCAL

GLOBAL

HUMANINTERFACE

HARDWARE ARCHITECTURE

MACHINE VISIONCOMPUTER

INTERFACE ANDARBITRATION

COMPUTERWIRELESS

HARDWARE

CENTRAL CONTROLAND

COMMUNICATIONS NETWORKCOMPUTER

VEHICLE(S)HIGH LEVEL

CONTROLCOMPUTER

VEHICLE

VEHICLE

VEHICLE(S)HIGH LEVEL

CONTROLCOMPUTER

HUMAN INTERFACECOMPUTER

WIRELESSHARDWARE

LOCAL LOCAL

HARDWARE PORT

HARDWARE PORT

HUMAN INTERFACECOMPUTER

SIMPLE COMMUNICATIONS NETWORK MODEL:

,

,

: communicating subsystem

: maximum bandwidth from to , data units/frame

: latency from to , frames

i

i j i j

i j i j

U i

B U U

L U U

Bi,j data unitsBi,j data units

buffer Li,j

Bi,j data units

buffer Li,j -1 buffer 0

Ui Uj

RoboFlag Simulation Environment

Hardware Environment

People

Michael Babish (Research Support) Andrey Klochko (Programmer) JinWoo Lee (Post-Doc) 30 UG and M.Eng. students

Shared platform with DARPA MICA

The RoboFlag Drill: Matt Earl

Defenders

( , ) ( )i i i id f d u u t U

( , ) ( )j j j ja g a v v t V Attackers

Constraints-defenders cannot enter defense zone

-no collisions

•Defender objective: pick u(t) to minimize number of attackers within defense zone at time T.•Attacker objective: pick v(t) to maximize number of attackers within defense zone at time T.

Problem: Consider the RoboFlag Drill with the following assumptions

•irrational attackers (drones) •full and perfect information•collisions are ok•simple 2nd order defender dynamics

Model as a discrete time mixed logical dynamical (MLD) system (Bemporad & Morari 1999)

•linear dynamics•logical rules•inequality constraints

Defenders

max max,

i i xi

i i yi

xi yi

x x u

y y u

u u u u

( ) ( ) ( ) ( )i i i ix b x b y b y b constraints (must not enter defense zone)

dynamics

intercept region

( ) {( , ) : ( ) ( )}i i iI t x y c x x c c y y c intercept region (dotted region in figure above)

Attackersauxiliary binary variables 1 ( , )ij j j ix y I

1 ( , )j j jx y G

dynamics

( 1) ( ) ( )j j j jr t r t v a t

Optimal control policy

we convert the system into MLD form using HYSDEL (Torrisi, Bemporad, and Mignone 2000)

,1

min ( )aN

ju

j

T

1 2

1 2 3 5

( 1) ( ) ( ) ( ), (0)

( ) ( ) ( )ox t Ax t B u t B t x x

E x t E u t E t E

min{ : }T

zf z Az b

this can be written as a mixed integer linear program (MILP)

Results

• MILP problem: 4040 integer variables, 400 continuous variables,13580 constraints• CPLEX solves in 244 seconds on Linux PIII 866MHz

Results

Evaluation of this approach for cooperative control

Good

Bad

• easy to model complex systems• easy to formulate complex tasks via a cost function to minimize and constraints to be satisfied• handles very general constraints• codes available for solving mixed integer programs• gives optimal strategy

• computationally intensive• constraints only enforced at discrete times. • does not exploit structure

A new approach to reduce computation

Exploit structure of the problem by considering the discrete and continuous parts separately.

Form an intercept tree

- Prune intercept tree- Find feasible paths within tree

Exploit motion primitives

Path Planning under Uncertainty:Myungsoo Jun

MAIN IDEAS:

• Construction of probability map from available data• Measurement data• A priori statistics of environments

• Convert the probability map to a directed graph• Path planning by solving shortest path problem in digraph

Capture uncertainty in information with a probabilistic approach

Probability Map Building• Measurement update by measured data

• Time update by a priori statistics of environment

• Map building

sensor characteristics

environment statistics

From this can obtain probability distribution that at least one opponent is at a certain location

Conversion to Digraph

1 2 3

4 5 6

7 8 9

0.02 0.015 0.01

0.013

0.001

0.02

0.02

0.015

0.015

0.0150.013

0.013

0.013

0.013

0.013

0.01

0.001

0.001

DigraphProbability Map

Examples

Dynamic Replanning Case with Multiple Vehicles

Simulation

Contribution and Future Work

• Consideration of time and velocity in path planning for multiple vehicles• Consideration penalty for frequent acceleration and deceleration• Improving map building computation for real-time application

• Building a probability map in uncertain dynamic environments • Path planning of multiple vehicles in uncertain dynamic environments based on probability maps• Integrating map building and path planning

Contribution:

Future Work:

Obstacle Avoidance in Dynamic Environments: Pritam Ganguly

Objective: Computationally fast algorithms for path planning in multi-agent adversarial environments with delayedinformation.

APPROACH•Game Theoretic: Avoiding a rational adversary in a delayedenvironment can be modeled as a non-cooperative imperfect information game . Trajectory generation is an outcome of such an approach.•Randomized Algorithm: This algorithm uses an existingtrajectory generation routine to generate feasible paths in the presence of obstacles. One way to incorporate the effect of delayis to associate with each obstacle a reachability regime over thedelayed steps. Frazzoli, Dahleh, and Feron

Randomized Algorithm

Terminology:

•Primary Node:

An equilibrium configuration belonging to the state-space of theagent.

•Secondary Node:

An element of the state space ofthe agent which lies on the pathfrom the initial point to a primary node.

Randomized Algorithm

Main Idea: (Frazzoli,Dahleh & Feron ‘00)

•The main idea is to search for random intermediate points in the state-space which might generate a feasible path to the destination. A feasible path being the one without any collisions.

•Among all the feasible paths the one with the lowest cost (eg. time) is then chosen.

•The underlying assumption in using this algorithm is that one already has a way of generating trajectories in the absence of obstacles.

Randomized Algorithm

Feasible PathFeasible Pathfound, updatefound, update

costscosts

Initialize treeInitialize treewith starting positionwith starting position

Randomly generate primaryRandomly generate primary node and also a start pointnode and also a start pointfrom the already generatedfrom the already generated

treetree

Is Path(start,primary)Is Path(start,primary)feasiblefeasible

yesyes

Generate random Generate random secondary points andsecondary points andadd them to the treeadd them to the tree

Is Path(secondary,Is Path(secondary,destination) feasibledestination) feasible

yesyes

NoNo

NoNo

Main Idea and Implementation:

•This algorithm is probabilistically complete: with probability one, it returns a feasible path if there exists one.

•Instead of using a tree data structure, use a grid data structure which takes a large storage space but has faster access time.

Future Work

• Implement the randomized algorithm framework for multiple agents in a centralized fashion, which would be a relatively easy extension to the present algorithm by increasing the state-space dimension.

• Develop a protocol enabling the decentralization of the above computation, and prove that the protocol achievesthe desired objective.

Atif Chaudhry

Survey published research on multiple vehicle control

Investigating relationship between RoboFlag framework and desired military capabilities of autonomous vehicles