Decision Making Under Uncertainty PI Meeting - June 20, 2001 Distributed Control of Multiple Vehicle...

Decision Making Under Uncertainty PI Meeting - June 20, 2001

Distributed Control of Multiple Vehicle Systems

Claire Tomlin and Gokhan Inalhanwith

Inseok Hwang Rodney Teo and Jung Soon Jang

Department of Aeronautics and AstronauticsStanford University


Motivation


• Aviation surveillance / imaging

• Search / Rescue / Disaster relief

• Precision Agriculture• Environmental Control

& Monitoring

• UCAV Fleets• Communication Relays

• Remote sensing / distributed data acquisition

Application Areas


Background: Multiple Aircraft Maneuvers

sin2u õ r2i +r2

j à 2r ir j cos(òià òj)R2

Safe if …


A Simple Protocol

90î ô î òminô 120î ) u > 1:56îCase 1:

60î ô î òminô 90î ) u > 2:20î

45î ô î òminô 60î ) u > 2:88î

30î ô î òminô 45î ) u > 4:26î

10î ô î òminô 30î ) u > 12:74î

0î ô î òminô 10î ^dminõ 3R ) u > 19:48î

Case 2:

Case 3:

Case 4:

Case 5:

Case 6:


3 aircraft collision avoidance


10 aircraft collision avoidance


Robust to Uncertainties in Position

However, current protocol is centralized, not robust to communication uncertainty


Game Theoretic Approach

xçi = vi cos(ì i) +ui sin(ì i)yçi = vi sin(ì i) à ui cos(ì i)ìçi = ! i +î ! i


Analytic Computation of Blunder Zone


Sample Trajectories

Segment 1

Segment 2

Segment 3


Application to Formation Flight

• possible for a two aircraft system

• what about multiple (>2) aircraft?


Directed Graph Example of FMS

B S E P(A=F) P(A=T)

F F F 0.99 0.01

T F F 0.3 0.7

F T F 0.2 0.8

F F T 0.15 0.85

T T F 0.1 0.9

T T T 0.01 0.99

T F T 0.05 0.95

F T T 0.03 0.97

Continuous behavior?


• Aircraft motion is presented with hybrid modes

• Provides a basis for embedding discrete decisions, finite dimensional optimization, discrete state propagation

• Reachability algorithms

Hybrid Model of Aircraft

STRAIGHTLEVEL

FLIGHT : MIN

V_minimum

STRAIGHTLEVEL

FLIGHT : MAX

V_maximum

STRAIGHTLEVEL

FLIGHT : CRUISE

V_cruise

COORDINATEDTURN LEFT

V_cruisew_max

COORDINATEDTURNRIGHT

V_cruise-w_max

V_cruise 63 m/sec

V_minimum 123 m/sec

V_maximum 90.6 m/sec

W_maximum 1.2 deg/sec


Hybrid Model of Aircraft

xçi = Vi cos i

yçi = Vi sin i

çi = ! i i = 1; . . .;M

• Continuous dynamics – planar kinematic model

• Our examples: hybrid model with five flight modes

Wiã

k

ith vehicle

kth step

optimal

variable

Vimin ô Vi ô Vi

max

à wimax ô wi ô wi

max

xik+1

= xik+

! i

k

Vi

k sin ik+ ! i

kTs

ð ñà sin i

k

ð ñh i

yik+1

= yik+

! i

k

Vi

k cos ik

ð ñà cos i

k+ ! i

kTs

ð ñh i

ik+1

= ik+! i

kTs

xik+1

= xik+ Vi

kcos i

k

ð ñh iTS

yik+1

= yik+ Vi

ksin i

k

ð ñh iTS

ik+1

= ik

Straight à Level FlightVi = Vset ;wi = 0

Coordinated TurnVi = Vset ;wi = wset


Example (continued)

Motion of Vehicle 1

Motion of Vehicle 2


t=0.2 min. t=0.4 min. t=0.6 min. t=0.8 min.

Vehicle 1

Vehicle 2

Trees of Possible Locations for each Vehicle


Mode Sequence 245 (base ten) : 1-4-4-0 (base five)Vmax for 0.1 min; Left Turn for 0.1 min; Left Turn for 0.1

min; Vcruise for 0.1min

Cost (from desired) vs. Mode Selection


Red SAFE

Blue UNSAFE

Matrix Game Structure for Hybrid Modes


No safe mode for vehicle #1 for every mode selection of vehicle #2

No safe mode for vehicle #2 for every mode selection of vehicle #1

Coordination is needed


Dynamic Coordination Problem

L aug(U; f õTli;õT

lk6=ig; f öT

li;öTlk6=i

g)

L augi(ui;õTli;öT

lijf xj; xjç;ujgi)

L aug(f ugk6=i; f õTl gk6=i; f öT

l gk6=ijf xi;xçi;uig)

f J 2; f l2;gl2;ui 2 P2g

f J 1; f l1;gl1;u1 2 P1g

f J 4; f l4;gl4;u4 2 P4g

f J 5; f l5;gl5;u5 2 P5g

f J i; f li;gli;ui 2 P ig

f J 3; f l3;gl3;u3 2 P3g

f f ci1;gci1g

f f l23;gl23g

f f c15;gc15g

f f c34;gc34g f f c35;gc35g

f f ci2;gci2g

Löaugi(uk6=i;õTlk6=i

;öTlk6=i

)


Local to the ith Vehicle

ui

uk

D i = f xj;xçj;ujgi

Si = f xi; xçi;uig

max

öTli

õ0

õTli

2Rdi

minui2P i

H augi(xi; xçi;ui;õTli;öT

li; [t; t + T]jD i)

max

föTlgkõ0

fõTlgk2Rdk

minfuk2P kg

P

k6=iH augk(xk; xçk;uk;õT

lk;öTlk; [t; t + T]jSi)

8k2f1;...;Mgk6=i

Local optimization by ith vehicle based on global information set Di

Group optimization by kth vehicle based on information set Si


INITIALIZEASSUMINGORIGINAL

FLIGHT PATHSOF ALL

VEHICLES

RECEIVEFLIGHT

SCENARIOSFROM OTHER

A/C

OPTIMIZEINDIVIDUAL FLIGHTPLAN FOR A GIVENFLIGHT SCENARIO

SET TIMER=0

STORESOLUTION

SELECT A FLIGHTSCENARIO FOR THESET OF AVAILABLE

SOLUTIONS

SEND FLIGHTSCENARIO TO

OTHER A/C

X-LINKCOMMUNICATION

RECEIVE

WHILE TIMER<T_ITERREPEAT

SEND

TIMER >= T_ITER

Decentralized Optimization


minUi2P i

J li xi;xi;çui; t; t+T[ ]à á

õ 0

subject to Fi(X;Xç;U; t; t+T[ ]) ô 0Gi(X;Xç;U; t; t+T[ ])) = 0

inter-vehicular constraintsIndividual state propagation

local cost function

LOCAL COORDINATION PROBLEM

Fi(X;Xç;U; t; t+T[ ]) =f li(xi;xi;çui; t; t+T[ ]) = 0f ci(xi;xi;çui; t; t+T[ ]j Di) = 0

ú

Gi(X;Xç;U; t; t+T[ ]) =gli(xi;xi;çui; t; t+T[ ]) ô 0gci(xi;xi;çui; t; t+T[ ]j Di) ô 0

ú

Di = fxj;xjç;ujçg 8j 2 Nilocal information set (neighborhood)

inter-vehicular constraintsLocal vehicle constraints

Local Optimization by each Vehicle


LOCAL HAMILTONIAN

L i uãld

i; (õãld

i)T; (öãld

i)TjDi

ð ñ= max

õTi

2 Rdi

öTi

õ 0

minuu2P i

Hi xi;xi;çui;õTi;öT

i; t; t+T[ ]jDi

ð ñ

LOCAL DECENTRALIZED OPTIMAL

Hi xi;xi;çui;õTi;öT

i; t; t+T[ ]jDi

ð ñ= J li xi;xi;çui; t; t+T[ ]

à á

+õTiFi(xi;xi;çui; t; t+T[ ]jDi) +öT

iGi(xi;xi;çui; t; t+T[ ]jDi)

Perspective of the ith vehicle


Result

• Our iterative algorithm based on local decentralized optimization converges to a global decentralized optimal solution

thus at each iteration

As L is bounded below by zero, convergence is guaranteed

L i(U; fõTli;õT

lk6=ig; föT

li;öT

lk6=ig) = L i(ui;õ

Tli;öT

lijfxj;xjç;ujgi) +Löi(uk6=i;õ

Tlk6=i

;öTlk6=i

)

L i(uãld

i; (õãld

li)T; (öãld

li)TjDãld

i) ô L i(ui; (õ

ãld

li)T; (öãld

li)TjDãld

i) 8i = 1; . . .;M

Löaugi(uk6=i;õTlk6=i

;öTlk6=i

jDi) = constant

L(fuãld

i;uãld

k6=ig; f(õãld

li)T; (õãld

lk6=i)Tg; f(öãld

li)T; (öãld

lk6=i)Tg) ô

L(fui;uãld

k6=ig; fõT

li; (õãld

lk6=i)Tg; föT

li; (öãld

lk6=i)Tg) 8i = 1; . . .;M

fuãld

ig= Uãgd ( ) L i(u

ãld

i; (õãld

i)T; (öãld

i)TjDãld

i)) ô L i(ui; (õ

ãld

i)T; (öãld

i)TjDãld

i))8i = f1; . . .;Mg


GLOBAL COORDINATION PROBLEM

minfUi2P ig

X

i=1

M


õ 0

subject to F(X;Xç;U; t; t+T[ ]) =^

i

Fi(X;Xç;U; t; t+T[ ]) ô 08i = 1; :::;M

G(X;Xç;U; t; t+T[ ])) =^

i

Gi(X;Xç;U; t; t+T[ ]) = 08i = 1; :::;M

GLOBAL LAGRANGIAN

CONDITION FOR CENTRALIZED GLOBAL OPTIMALITY

L Uã; (õã)T; (öã)TjDià á

= maxõT 2 Rd

öT õ 0

minU2P

J global(U) +õTF(X;Xç;U; t; t+T[ ]) +öTG(X;Xç;U; t; t+T[ ])

L(f Uã1;Uã

2; . . .;Ui; . . .;U

ãM

g;õãT;öãT) õ L(f Uã1;Uã

2; . . .;Uã

i; . . .;Uã

Mg;õãT;öãT)

8i 2 f 1; . . .;Mg

Global Perspective


The global decentralized optimal solution corresponds to a Nash Equilibria of the centralized optimization problem for an M-player game with each player cost function corresponding to

and the constraints to


F(X;Xç;U; t; t+T[ ]) =V

iFi(X;Xç;U; t; t+T[ ]) ô 08i = 1; :::;M

G(X;Xç;U; t; t+T[ ]) =V

iGi(X;Xç;U; t; t+T[ ]) = 08i = 1; :::;M

Nash Equilibrium


AIRCRAFT #1450mph ô V ô 600mph

à 2deg=sec ô w ô 2deg=sec







C1=0.7 C2=0.8 C3=0.6 C4=0.9

Example: 4 Vehicle Coordination


• Each aircraft penalizes its own deviation from its desired flight path subject to– Minimum safety constraints (penalty functions)– Aircraft dynamics and flight modes (state propagation)

Local optimization given the constraint “information set”: {xj,yj,uj}i

min

fwi 2 Pwig

f Vi 2 PVigJ i

ììf xj; yjgi =

X

i=1

M

cTi

íí xi; yi( ) à xdes

i ; ydesi

à áííí

2+

X

j6=ij=1

M

cTij PF(xi; yi;uijf xj; yj;ujgi)

subject to G i(X ;U; t) = 0

xk+1i = xk

i + ! ki

Vki sin k

i + ! ki Ts

à áà sin k

i

à áâ ã

yk+1i = yk

i + ! ki

Vki cos k

i

à áà cos k

i + ! ki Ts

à áâ ã

k+1i = k

i + ! ki Ts

8>>><

>>>:

Example: 4 Vehicle Coordination


PF(xi; yi;uijf xj;yj;ujgi) =ûij 2ù

p1 eà 2 ûij

1íí x i(Vi;wi);yi(Vi;wi)( )à x j;yj( )

íí

2

PF(xi;yi;uijf xj;yj;ujgi) =íí F i (X ;U; t)

íí

1

• Approximate Penalty Function:

• Exact Penalty Function:

F i;j(X ;U; t) ô 0íí xi;yi( ) à xj; yj( )

íí

2 õ R2min 8i; j = 1; . . .;M

ni6=j

Penalty Methods


min

fwi 2 Pwig

f Vi 2 PVigJ global =

X

i=1

M

cTi

íí xi; yi( ) à xdes

i ;ydesi

à áíí

2

subject to F i;j(X ;U; t) ô 0íí xi;yi( ) à xj;yj( )

íí

2 õ R2min 8i; j = 1; . . .;M

ni6=j

G i(X ;U; t) = 0

xk+1i = xk

i + ! ki

Vki sin k

i + ! ki Ts

à áà sin k

i

à áâ ã

yk+1i = yk

i + ! ki

Vki cos k

i

à áà cos k

i + ! ki Ts

à áâ ã

k+1i = k

i + ! ki Ts

8>>><

>>>:

8i = 1; . . .;M

min

fwi 2 Pwig

fVi 2 PVigJ global =

X

i=1

M

cTi

íí xi(Vi;wi);yi(Vi;wi)( ) à xdes

i ; ydesi

à áíí

2 +

\

j6=ij=1

M

cTij PF(xi; yi;uijf xj; yj;ujgi)

• State propagation and safety constraints are naturally embedded in the cost function

Global Optimization


Aircraft # 1 Aircraft # 3

Aircraft # 2Aircraft # 4

WORLDMODEL

RBNB Server

TCP-IP

TCP-IP

TCP-IP

TCP-IP

TCP-IPTCP-IP

RBNB Matlink

Local ControlProcess

Client/ServerLayer

Testbed #1: Networked Simulation


Example 1


Example 2


Iteration Results


Dynamic Horizon

• Pointwise optimal control law is easily outperformed

• Global decreasing trend for – total coordination cost – constraint violation


Example: Multiple Vehicle Mission Design


Decentralized Initialization Procedure Heuristics– Multiple-Depots(Vehicles), Time-windows for access, Priority on

objectives and the vehicles– Iterative selection process carried via each vehicle– Best solution then selected from each vehicle’s solution set

Multiple Vehicle Mission Design


• 3 Dimensional Perspective– The tubes represent 2.5 km

radius safety zones– X[km] * Y[km] * Time[min]

Higher Dimensions


Testbed #2: Stanford DragonFly Test Platform

DragonFly Aircraft Aircraft New Airframe


DragonFly Avionics

Single-board Computer GPS board

IMU

Air-Data Probe

Ts

Ts

Ts

Tc

Control Command

Actuator Control Computer

Servo Control

• Vehicle ControlVehicle Control

• NavigationNavigation

• Path PlanningPath Planning

• Data Logging Data Logging

• CommunicationCommunication

• … … …


Software Architecture

IMU

GPS

DAQ

ACC

Radio(Modem/Ethernet)

Hardware

Serial

Serial

PC104

Serial

SerialTCP/IP

Navigation

SharedMemory

Nav.

DAQ

ACC(in)

RadioLink

ACC(out)

Nav.Nav.

Hi-rateDatalogger

Inner LoopControl

Planner /Outer Loop

Control

CriticalDisplay

downlink

DataLinkTransmitter

Clients Servers

MsgSnd() toother nodes

CriticalDisplayGround


Directions

• Application of algorithm directly to probabilistic hybrid models (Koller)

• Numerical implementation issues (Saunders)

• Evolution of the algorithm in a dynamic environment (connect operator)

• Dynamic visitation problems

Decision Making Under Uncertainty PI Meeting - June 20, 2001 Distributed Control of Multiple Vehicle...

Documents

Transcript of Decision Making Under Uncertainty PI Meeting - June 20, 2001 Distributed Control of Multiple Vehicle...