temporally and spatially deconflicted path planning for multiple marine vehicles

Temporally and Spatially Deconflicted Path Planning for Multiple Marine Vehicles

A. Häusler1, R. Ghabcheloo2, A. Pascoal1, A. Aguiar1

I. Kaminer3, V. Dobrokhodov3

1Instituto Superior Técnico, Lisbon, Portugal2Tampere University of Technology, Tampere, Finland

3Naval Postgraduate School, Monterey, California

Andreas J. Häusler - MCMC 2009 2

Introduction

Challenges in underwater environments can be overcome through the use of fleets of heterogeneous vehiclesCentral to cooperative control systems are efficient algorithms for multiple vehicle path planningExample: Go-To-Formation maneouvre

September 18th, 2009


The Go-To-Formation Maneouvre

An initial formation pattern must be established before mission startDeploying the vehicles cannot be done in formation (no hovering capabilities)Vehicles can’t be driven to target positions separately (no hovering capabilities)


Mother Ship

Cur

rent


The Go-To-Formation Maneouvre

Need to drive the vehicles to the initial formation in a concerted mannerEnsure simultaneous arrival times and equal speedsEstablish collision avoidance through maintaining a spatial clearance


Mother Ship


Path Planning System


MULTIPLE VEHICLEPATH PLANNING

SYSTEM

Initial Positions

Initial Velocities

Final Positions

Final Velocities

Cost Criterion (e.g. weighted sum of

energies, maneouvring time)

Vehicle dynamical constraints

Collision avoidance constraints

External constraints (e.g.

Obstacles)

Nominal Pathsand

Speed Profiles


Path Planning Foundations


0 0( ), ( )p t v t

( ), ( )f fp t v t

Initial position

Final position

( ( )), ( ( )), ( ( ))i i ip t v t a t


Path Planning Foundations

Dispense with absolute time in planning a path (Yakimenko)

Establish timing laws describing the evolution of nominal speed with Spatial and temporal constraints are thereby decoupled and captured by &Choose and as polynomialsPath shape changes with only varying


( )t

( ) [ ( ), ( ), ( )]p x y z

( )p ( ) d dt

( ) ( )p

f

[0, ]f


Path Planning for Single Vehicles

Polynomial for each coordinate with degree determinded by number of boundary conditionsTemporal speed and acceleration constraints

Choice for timing law with


0( ) N k

xkkx a

0 0ff

τη τ = η + η τ ητ

2 2 2( ) ( ) ' ( ) ' ( ) ' ( ) ( ) '( )v x y z p 2( ) ''( ) ( ) '( ) '( ) ( )a p p

(0) (0)v ( ) ( )f fv t


Path Planning for Single Vehicles

A path is feasible if it can be tracked by a vehicle without exceeding , , and

It can be obtained by minimizing the energy

consumption ,subject to temporal speed and acceleration constraints


minv maxv maxa

33

0

( ) '( )f

f DJ c c p d

max


Optimization for Multiple Vehicles

Instead of trying to minimize the arrival time

interval, we can fix arrival times to be exactly

equal and still get different path shapes

Integrate for


( )

(0) ( ) (0)

( ) (0)( ) (0)

ln( ( ) (0))

i

ii

i

i

i f i f i

i f ifi f i

i f i

t


Optimization for Multiple Vehicles

Then we obtain ,

from which the spatial paths are computedThe result is in turn used to compute velocity and accelerationOptimization is done using direct search


( ) (0)

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

( ) ( ) (0)

i i

f

i i

f f i f i

tti i fi

f i f ii f i f i

t t

t


Spatial Deconfliction


1 0 1 0( ), ( )p t v t

2 0 2 0( ), ( )p t v t

1 1( ), ( )f fp t v t

2 2( ), ( )f fp t v t

d E

Initial positions

Final positions(target formation)


Temporal Deconfliction


1 0 1 0( ), ( )p t v t

2 0 2 0( ), ( )p t v t

1 1( ), ( )f fp t v t

2 2( ), ( )f fp t v t

Initial positions

Final positions(target formation)


Deconfliction

Spatial deconfliction: subject to

For temporal deconfliction, the constraint changes to


; 1, , 1

mini

n

i ii n i

w J

2 0i k j lp τ p τ E ;E >

2 0i jp t p t E ;E >

1, 0, fi, j = ,n,i j,t t

1, 0, 0,k l f fi ji, j = ,n,i j, τ ,τ τ τ


Deconfliction

Simultaneous arrival at time

Time-coordinated path following using virtual time with


1 2

opt

, 1

arg minf

n

f i it t t i

t w J

0,1ft t

( ) (0)

ln 1 ( ) (0) 1( ) (0)

ln ( ) (0)

i i

i i

i

i

i f i f i

i f i i ffi f i

i f i

t t

(Ghabcheloo, ACC 2009)


Simulation Results

Spatial deconfliction in 2D for three vehicles – paths and velocity profiles



Simulation Results

Temporal deconfliction in 2D for three vehicles – paths and velocity profiles



Simulation Results

Spatial deconfliction in 3D for three vehicles – paths and velocity profiles



Simulation Results

Temporal deconfliction in 2D, facing a current of 0.5 m/s coming from the eastAdditional optimization criterion: final velocity to match desired value of 1.5 m/s



Conclusions

Multiple vehicle path planning techniques based on direct optimization methodsFlexibility in time-coordinated path following through decoupling of space and timeNo complicated timing laws – constraints are incorporated in spatial description (e.g. initial and final heading)Suitable for real-time mission planning thanks to fast algorithm convergence



Future Work

Show robustness of algorithm through extensive simulationsCompare our results with results from optimal controlEmploy path planning on vehicle hardware (GREX, Co3-AUVs) for sea trialsIncorporate avoidance of fixed obstaclesIntegrate constraints imposed by cooperative path following (e.g. communication links)



Thank you for your attention!


Delfim (IST/ISR) Infante (IST/ISR)

ASTERx (IFR)

Seawolf (ATL)

Arquipélago (IMAR) DelfimX (IST/ISR)

temporally and spatially deconflicted path planning for multiple marine vehicles

Documents