Multiple Vehicle Motion Planning: An Infinite Diminsion Newton Optimization Method

LARSyS Summer School, July 10, 2014

Multiple Vehicle Motion Planning

An Infinite Dimension Newton Optimization Method

Andreas J. HäuslerLaboratory of Robotics and Systems in Science and Engineering

Instituto Superior Técnico

Why do we need to plan?

• Efficient algorithms for multiple vehicle path planning are crucial for

cooperative control systems

• Should take into account vehicle dynamics, mission parameters and

external influences to allow for accurate tracking

• Usually allows to specify optimization criteria such as minimum

energy usage

• Example: Go-To-Formation maneuver

Introduction

Problem Setting

Go-To-Formation Maneuver

Literature

Contribution

Path Planning

Minimum Energy

PRONTO

Simulation Results

Conclusion

2 of 123


Introduction

Problem Setting


Literature

Contribution

Path Planning

Minimum Energy

PRONTO

Simulation Results

Conclusion

Current

• An initial formation pattern must be established before mission start

• Deploying the vehicles cannot be done in formation (no hovering capabilities)

• Vehicles can’t be driven to target positions separately (no hovering capabilities)

3 of 123


Introduction

Problem Setting


Literature

Contribution

Path Planning

Minimum Energy

PRONTO

Simulation Results

Conclusion

• Need to drive the vehicles to the initial formation in a concerted manner

• Ensure simultaneous arrival at equal speeds on temporally deconflicted trajectories

• Establish collision avoidance through maintaining a spatial clearance

4 of 123

Cooperative Planning in the Literature

The vast majority of approaches use heuristics/simplifying assumptions:

• Pseudospectral Method: problem is discretized at Legendre-Gauss-

Lobatto quadrature points and interpolated in between [Lewis, Ross, and Gong 2007]

• Dubins Paths: vehicle dynamics are reflected as value constraints on

acceleration or curvature and the resulting paths are not differentiable [Lee

and Kim 2007]

• A* Search: requires the environment to be discretized [Francis, Annavitti, and Garrett 2013]

• Cellular Automaton based Planning: again, environment needs to be

discretized, plus vehicle motion is treated discrete [Iaonnidis, Sirakoulis, Georgios, and

Andreadis 2011]

• Mixed Integer Linear Programming: time discretization [Kuwata and How 2011]

• Sequential Quadratic Programming: e.g. on B-spline coefficients, limited

to differentially flat systems [Lian 2008]

• …

Introduction

Problem Setting


Literature

Contribution

Path Planning

Minimum Energy

PRONTO

Simulation Results

Conclusion

5 of 123

Contribution

• Explicitly incorporated nonlinear vessel dynamics

• Four-quadrant thruster model for energy consumption and

propulsion calculation

• Using a descent method for solving constrained continuous-time

optimal control problems

• Pre-planners for collision avoidance and terrain-based trajectory

generation

Introduction

Problem Setting


Literature

Contribution

Path Planning

Minimum Energy

PRONTO

Simulation Results

Conclusion

6 of 123

Polynomial-based Path PlanningConstant Velocity, Deconfliction, and Direct Search.

Introduction

Path Planning

Minimum Energy

PRONTO

Simulation Results

Conclusion

7 of 123

Path Planning Foundations

Introduction

Path Planning

Foundations

Single Vehicle

Multiple Vehicles

Deconfliction

TCPF

Simulation Results

Summary

Minimum Energy

PRONTO

Simulation Results

Conclusion0 0( ), ( )p t v t

( ), ( )f fp t v t

Initial position

Final position

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

8 of 123

Path Planning Foundations

• Dispense with absolute time in planning a path [Yakimenko, 2000]

• Establish timing laws describing the evolution of nominal speed

with

• Spatial and temporal constraints

are thereby decoupled and

captured by and

• Choose and as

polynomials

• Path shape changes with only

varying

Introduction

Path Planning

Foundations

Single Vehicle

Multiple Vehicles

Deconfliction

TCPF

Simulation Results

Summary

Minimum Energy

PRONTO

Simulation Results

Conclusion

( )t

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

( )p ( ) d dt

( ) ( )p

f

[0, ]f

9 of 123

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

Introduction

Path Planning

Foundations

Single Vehicle

Multiple Vehicles

Deconfliction

TCPF

Simulation Results

Summary

Minimum Energy

PRONTO

Simulation Results

Conclusion

minvmaxv maxa

max

2

0 0

1( ) ( ) ( ) ( ) ( )

2

f f

w D w w wJ D T v t dt c v t A ma t v t dt

cv

totv

wvT

L

D

H C

10 of 123

Brachistochrone problem

Introduction

Path Planning

Foundations

Single Vehicle

Multiple Vehicles

Deconfliction

TCPF

Simulation Results

Spatial and Temporal Deconfliction

Ocean Currents and Brachisto-chroneProblem

Summary

Minimum Energy

PRONTO

Simulation Results

Conclusion

Solution of the classical Brachistochrone problem [A. Bryson & Y.-C. Ho 1975]. Initial and finalheading were part of the design variables. (Nearly) constant velocity was achieved byusing the non-polynomial form of η(τ).

11 of 123

Final Facts

• Multiple vehicle path planning techniques based on direct

optimization methods

• Flexibility in time-coordinated path following through decoupling of

space and time

• No 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

• Simulations demonstrate that results are as good as those obtained

analytically from optimal control

Introduction

Path Planning

Foundations

Single Vehicle

Multiple Vehicles

Deconfliction

TCPF

Simulation Results

Final Facts

Minimum Energy

PRONTO

Simulation Results

Conclusion

12 of 123

The Minimum Energy ProblemPlanning framework and vehicle modeling.

Introduction

Path Planning

Minimum Energy

PRONTO

Simulation Results

Conclusion

13 of 123

Problem Setting

• The polynomial-based path planning approach does not allow for

accurate predictions of the actual energy used

• In fact, the power integral

turns out to be invalid in some cases

Introduction

Path Planning

Minimum Energy

Problem Setting

Main Features

Planning Framework

Vehicle Model

Propeller Theory

Optimization Problem

PRONTO

Simulation Results

Conclusion

2 3

0 0 0

1( ) ( ) ( ) ( ) ( )

2

ff f

w w wD w wJ D T v t dt C v t A ma t v t dt v t dt

14 of 123

Classical Energy Computation

I. Steady Motion (no current)

• Simplified, and

• Then, instantaneous power is

• Therefore, energy consumed is

Introduction

Path Planning

Minimum Energy

Problem Setting


Main Features

Planning Framework

Vehicle Model

Propeller Theory


PRONTO

Simulation Results

Conclusion

Thrust TBall in water

Speed v

Drag D

T D

3P Tv kv

3E kv dt

2D kv

15 of 123


II. Motionless in the presence of current

• Now,

• However, the energy spent clearly has to be !

Introduction

Path Planning

Minimum Energy

Problem Setting


Main Features

Planning Framework

Vehicle Model

Propeller Theory


PRONTO

Simulation Results

Conclusion

0Tv

0E


Speed v

Drag D

Current vc

v=0

D=0

16 of 123


III. Non-steady motion

• Acceleration

• Energy spent is now

• Problem: over a given time interval, the integration over the term

may be 0 in cases where energy is spent!

Introduction

Path Planning

Minimum Energy

Problem Setting


Main Features

Planning Framework

Vehicle Model

Propeller Theory


PRONTO

Simulation Results

Conclusion


Speed v

0d

a vdt

3E kv mav dt

mav

17 of 123

Solution: Include motor equations

• Now: electrical power

• Energy expenditure is

• Use optimal control techniques to do trajectory planning,

incorporating explicitly the full vehicle dynamics

• This requires modeling of the vessel, the motors, and the propulsion

system

Introduction

Path Planning

Minimum Energy

Problem Setting


Main Features

Planning Framework

Vehicle Model

Propeller Theory


PRONTO

Simulation Results

Conclusion

Thrust TBattery Pack D/C Motor Propeller

V I

E P t dt

18 of 123

Main Features

• Vision: Groups of autonomous vehicles freely roaming the oceans

• Objectives: Acquire data on an unprecedented scale; detect and

monitor episodic events; inspect critical infrastructure on permanent

basis

• Requirement: Planning a mission that can be properly executed with

minimal energy expenditure

• Challenges: Simultaneous planning for several vehicles; possibly

heterogeneous team configuration; inter-vehicle and obstacle collision

avoidance; spatial team configuration; …

Introduction

Path Planning

Minimum Energy

Problem Setting


Main Features

Planning Framework

Vehicle Model

Propeller Theory


PRONTO

Simulation Results

Conclusion

19 of 123

Planning Framework

Introduction

Path Planning

Minimum Energy

Problem Setting


Main Features

Planning Framework

Vehicle Model

Propeller Theory


PRONTO

Simulation Results

Conclusion

Initial Curves

Initial State & Input

Final State & Input

Expected Energy

Trajectories

Safety Distance

Communication Constraint

Cost Criterion

Current

Obstacles

Desired Trajectory

Bat

hym

etri

c D

ata

Projection Operator based Newton method for Trajectory Optimization

(PRONTO)

Mission Specifications

Co

ord

inat

ed T

raje

ctor

y T

rack

ing

Co

ntr

olle

r

Environmental Constraints

Su

per

vis

ing

Mis

sio

n O

per

ato

r

Dynamics

Coordinated Paths

AMV Data

Pre-Planner

Desired Trajectory

Mission Specifications

Pre-Planner

20 of 123

Planning Framework

• In what follows, simplicity of the presentation is maintained by not

including ocean currents and restricting planning to planar motions

• Not problematic: incorporating ocean currents is straightforward, as

would be adapting the framework to an existing 3D version of the

planner [Saccon et al. 2012]

Introduction

Path Planning

Minimum Energy

Problem Setting


Main Features

Planning Framework

Vehicle Model

Propeller Theory


PRONTO

Simulation Results

Conclusion

21 of 123

Vehicle ModelingThe MEDUSAS, its motors, and its thrusters.

Introduction

Path Planning

Minimum Energy

Problem Setting

Vehicle Model

Propeller Theory


PRONTO

Simulation Results

Conclusion

22 of 123

The MEDUSASSemi-SubmersiblePropulsion system consists of

two Seabotix HPDC 1507

thrusters. The vessel is steered

via differential thrust.

Maximum speed is 1.5 m/s.

Introduction

Path Planning

Minimum Energy

Problem Setting

Vehicle Model

Propeller Theory


PRONTO

Simulation Results

Conclusion

23 of 123

Dynamic Model

• Assumption: since the MEDUSA is a “surface” craft, we restrict the

motions to 2D (i.e., ignore roll and pitch dynamics)

• Propulsion and steering with common and differential thrust (two

thrusters, no rudders) via port side and starboard propeller speed

inputs (rate of change)

• This gives us 3 kinematic states + (3+2) dynamic states + 2 inputs

Introduction

Path Planning

Minimum Energy

Problem Setting

Vehicle Model

Dynamics

Motors

Thrusters

Propeller Theory


PRONTO

Simulation Results

Conclusion

24 of 123

Dynamic Model

• Kinematics:

• Dynamics:

with and

• Inputs: rotational acceleration of port side and starboard propeller

Introduction

Path Planning

Minimum Energy

Problem Setting

Vehicle Model

Dynamics

Motors

Thrusters

Propeller Theory


PRONTO

Simulation Results

Conclusion

cos sin 0

sin cos 0

0 0 1

u

v

r

x

y

rbrb aa nC C D DM M

u

v

r

ps sb

ps sb

0

T T

l T T

25 of 123

D/C Motor Model

• Standard D/C motor equations

• Motor armature inductance is , i.e., effect of inductance is

negligibly small compared to motor motion

safe to assume that (fast dynamics)

equation for voltage: steady-state

• Quasi-static model for electrical current

Introduction

Path Planning

Minimum Energy

Problem Setting

Vehicle Model

Dynamics

Motors

Thrusters

Propeller Theory


PRONTO

Simulation Results

Conclusion

a e

m t a hyd

a

J b

dL I R I V Kd

K

t

I Q

a 0ddtL I

a hyd

t

1I b Q

K

a 500μHL

a eV R I K

26 of 123

D/C Motor Model

• Motor torque coefficient and viscous friction coefficient are

obtained by nonlinear least squares fit to measurement data

• Total instantaneous power requirement is then

Introduction

Path Planning

Minimum Energy

Problem Setting

Vehicle Model

Dynamics

Motors

Thrusters

Propeller Theory


PRONTO

Simulation Results

Conclusion

ps ps sb sb payloadV I V I PP

b

a a,I v u

tK

Armature current for “slices” of the advance velocity

27 of 123

Thruster Model

• Due to design: negligible

propeller-hull interaction

• Four-quadrant propeller model

where

• With inputs and

Introduction

Path Planning

Minimum Energy

Problem Setting

Vehicle Model

Dynamics

Motors

Thrusters

Propeller Theory


PRONTO

Simulation Results

Conclusion

ps

sb

ps

sb

a

a

p

sbp

ps0.7

0.7

v lr u

v lr u

v R

v R

ps

2 2T a p

2 2Q a p

2

2

1

2

1

2

T c v v

Q c v v

R

R d

sb

28 of 123

Propeller ModelingThe classical open-water model, the four-quadrant model, and its improvement.

Introduction

Path Planning

Minimum Energy

Problem Setting

Vehicle Model

Propeller Theory


PRONTO

Simulation Results

Conclusion

29 of 123

Why a sophisticated propeller model?

• Mostly, energy consumption is computed with mechanical

considerations only

• For non-conservative (i.e., non-zero forward-only) motion, this

exhibits certain problems

• In addition, this approach does not allow for conclusive

knowledge about the “real” energy taken from the batteries along

a given trajectory

Introduction

Path Planning

Minimum Energy

Problem Setting

Vehicle Model

Propeller Theory


PRONTO

Simulation Results

Conclusion

30 of 123

Propeller Basics

Introduction

Path Planning

Minimum Energy

Problem Setting

Vehicle Model

Propeller Theory

Propeller Basics

The Four Quadrants of Operation

The H-Model

The L-Model

Passivity

Results


PRONTO

Simulation Results

Conclusion

va

vtot

va

vp

• Classically, propellers are mathematically described in terms of the

advance velocity and the propeller speed

• Their relation at the propeller blade defines the

advance angle ,

• making use of the tangential velocity

of the propeller,

av

( , )a patan2 v v

0.7pv R

31 of 123

Propeller Basics

• Propeller models allow for the computation

of thrust and torque, and lift and drag forces

Introduction

Path Planning

Minimum Energy

Problem Setting

Vehicle Model

Propeller Theory

Propeller Basics


The H-Model

The L-Model

Passivity

Results


PRONTO

Simulation Results

Conclusion

vpvtot

va

T

L

D

/ 0.7Q R

32 of 123

The four quadrants of operation

• The advance angle defines four quadrants of propeller operation

Introduction

Path Planning

Minimum Energy

Problem Setting

Vehicle Model

Propeller Theory

Propeller Basics


The H-Model

The L-Model

Passivity

Results


PRONTO

Simulation Results

Conclusion

CAMS, 9-17-2013

ahead

crash-back

back

crash-ahead

9

0

0

0 0°

av

90 180

0

0

°

av

180 270

0

0

°

av

270 360

0

0

°

av

vpvtot

va

vp

vtot

va

vp

va

vtot

vpvtot

va

33 of 123

9

0

0

0 0°

av

90 180

0

0

°

av

180 270

0

0

°

av

270 360

0

0

°

av

The four quadrants of operation


• The classical open-water propeller model only covers the first

quadrant

Introduction

Path Planning

Minimum Energy

Problem Setting

Vehicle Model

Propeller Theory

Propeller Basics


The H-Model

The L-Model

Passivity

Results


PRONTO

Simulation Results

Conclusion

ahead

crash-back

back

crash-ahead

5

24

2

2

2

2

( )

( )

T o

Q o

ao

d k J

d k

T

Q

J

J

v

d

vpvtot

va

34 of 123

The four-quadrant propeller model


• The classical open-water propeller model only covers the first

quadrant

• In 1969, van Lammeren et al. developed the four-quadrant propeller

model [van Lammeren et al. 1969], [Oosterveld 1970]

Introduction

Path Planning

Minimum Energy

Problem Setting

Vehicle Model

Propeller Theory

Propeller Basics


The H-Model

The L-Model

Passivity

Results


PRONTO

Simulation Results

Conclusion

2 2 2

2 2 2

( )

(

0.5

0.5 )

( , )

Q

c v v R

c v v R

T

Q d

v v

T a p

a p

a patan2

vpvtot

va

35 of 123

The four-quadrant propeller model

• The van Lammeren et. al

propeller model (“Wageningen

series”) is given as 20th order

Fourier series

• Coefficients are based on a

fitting of measurements,

conducted with, and available

for, several propeller types.

Introduction

Path Planning

Minimum Energy

Problem Setting

Vehicle Model

Propeller Theory

Propeller Basics


The H-Model

The L-Model

Passivity

Results


PRONTO

Simulation Results

Conclusion

0

0

cos ( )sin

cos ( )

( )

( ) sin

T T

Q Q

T

Q

m

c ck

m

c ck

c A k

c

k B k k

k B kk kA

[Excerpt from page 24 of van Lammeren et al.’s seminal publication.]

36 of 123

The four-quadrant model of Healey et al.

• Our optimizer requires the dynamics to be

• Not the case for open-water model

• van Lammeren et al.’s model fulfills that requirement, but turns

out to be problematic during optimization

Introduction

Path Planning

Minimum Energy

Problem Setting

Vehicle Model

Propeller Theory

Propeller Basics


The H-Model

The L-Model

Passivity

Results


PRONTO

Simulation Results

Conclusion

2

37 of 123

vpvtot

va

• Developed in 1994, this model defines propeller lift and drag

coefficients as [Healey et al. 1994]

• Lift and drag are related to thrust and torque through rotation

about :

• This gives the thrust and torque coefficients

The four-quadrant model of Healey et al.

Introduction

Path Planning

Minimum Energy

Problem Setting

Vehicle Model

Propeller Theory

Propeller Basics


The H-Model

The L-Model

Passivity

Results


PRONTO

Simulation Results

Conclusion

( ) sin 2

( (1) cos2 ) / 2

maxL L

maxD D

c c

cc

H

H

cos sin

sin co / (0.7 Rs )

L T

D Q

( ) ( )cos ( )sin

0.7( ) ( )sin ( )cos

2

T L D

Q L D

c c

c c

c

c

H H H

H H H

38 of 123

A closer look

• The H-model may fail to capture physical constraints:

1. Lift and drag curves go through 0 at the same angle of attack

Introduction

Path Planning

Minimum Energy

Problem Setting

Vehicle Model

Propeller Theory

Propeller Basics


The H-Model

The L-Model

Passivity

Results


PRONTO

Simulation Results

Conclusion

39 of 123

A closer look



2. This propagates to thrust and torque, which are 0 at exactly the

same advance angle

Introduction

Path Planning

Minimum Energy

Problem Setting

Vehicle Model

Propeller Theory

Propeller Basics


The H-Model

The L-Model

Passivity

Results


PRONTO

Simulation Results

Conclusion

40 of 123

A closer look



2. This propagates to thrust and torque, which are 0 at exactly the

same advance angle

3. In the drag polar, we see that a residual drag component is

missing in the H-model

Introduction

Path Planning

Minimum Energy

Problem Setting

Vehicle Model

Propeller Theory

Propeller Basics


The H-Model

The L-Model

Passivity

Results


PRONTO

Simulation Results

ConclusionH-model W-model

( )c L

( )c D

41 of 123


4. Efficiency of the H-model does not exhibit the usual discontinuity

A closer look

Introduction

Path Planning

Minimum Energy

Problem Setting

Vehicle Model

Propeller Theory

Propeller Basics


The H-Model

The L-Model

Passivity

Results


PRONTO

Simulation Results

Conclusion

crash-ahead

crash-backback

ahead

a

2

o

v

dJ

aTv

Q

H-modelW-model

a

2

o

v

dJ

a 0v 0T

0Q

0Q

0 0since and simultaneously, and because

of L'Hôpital's rule, the -m finite everywhodel is e re

T Q

H

a 0v

0 means we go from to

operation; the actual “switch”

occurs at the s

ah

in

b

g

ack

ula t

a

y

e d

ri

no different curves for

because of propeller symmetry

42 of 123

The L-model

• Four our L-model, lift and drag coefficients are

• As with the H-model, we compute the L-model thrust and torque

coefficients through rotation about the advance angle

• The parameters are obtained by using a nonlinear LS problem that

a) captures the characteristics of first-quadrant efficiency

b) approximates a given (e.g., Wageningen series) and

inside the main operating region

c) enforces monotonicity of produced thrust

Introduction

Path Planning

Minimum Energy

Problem Setting

Vehicle Model

Propeller Theory

Propeller Basics


The H-Model

The L-Model

Passivity

Results


PRONTO

Simulation Results

Conclusion

min

( ) sin 2( )

( ) 1 cos2( ) / 2

maxL L

max minD D D D

L

Dc

c o

c

c

c o c

L

L

( )T

( )Tc

0 50 °

( )Qc

vpvtot

va

43 of 123

Analysis

1. Open-water efficiency shows expected behavior

Introduction

Path Planning

Minimum Energy

Problem Setting

Vehicle Model

Propeller Theory

Propeller Basics


The H-Model

The L-Model

Passivity

Results


PRONTO

Simulation Results

Conclusion

H-model L-modelW-model

back

ahead

Jo Jo Jo

( )J o

crash-ahead

crash-back

44 of 123

Analysis


2. Produced thrust is a monotone curve

Introduction

Path Planning

Minimum Energy

Problem Setting

Vehicle Model

Propeller Theory

Propeller Basics


The H-Model

The L-Model

Passivity

Results


PRONTO

Simulation Results

Conclusion

( )T

T

Q

n n n

H-model L-modelW-model

( )J o

45 of 123

Analysis


2. Produced thrust is a monotone curve

3. Efficiency is non-ideal and close to original four-quadrant model

Introduction

Path Planning

Minimum Energy

Problem Setting

Vehicle Model

Propeller Theory

Propeller Basics


The H-Model

The L-Model

Passivity

Results


PRONTO

Simulation Results

Conclusion

ideal efficiency (momentum theory)Wageningen series propeller modelHealey et al.’s approximationL-modelscaled data from MARIUS vehicle

2/k JT o

( )T

( )J o

46 of 123

The propeller as power converter

• Each value of relates to two possible values of the advance angle

Introduction

Path Planning

Minimum Energy

Problem Setting

Vehicle Model

Propeller Theory

Propeller Basics


The H-Model

The L-Model

Passivity

Results


PRONTO

Simulation Results

Conclusion

oJ

crash-ahead

crash-back

back ahead

�� > 0, windmilling

�� < 0, windmilling

Tv

Q

a

Jo

47 of 123


• Therefore, plotting the efficiency over 4 quadrants in terms of the

advance ratio requires two curves to cover the entire range of

possible propeller operation

• These curves are identical for the H-model and the L-model (perfectly

symmetric propeller models), but not the original W-model

Introduction

Path Planning

Minimum Energy

Problem Setting

Vehicle Model

Propeller Theory

Propeller Basics


The H-Model

The L-Model

Passivity

Results


PRONTO

Simulation Results

Conclusion

oJ

48 of 123


• The singularity in the efficiency curve relates to the fact that the

propeller is a non-ideal power converter: a two-port static system that

must satisfy the dissipation inequality

• When , the engine is doing work on the propeller, and

must hold; the propeller is windmilling

Introduction

Path Planning

Minimum Energy

Problem Setting

Vehicle Model

Propeller Theory

Propeller Basics


The H-Model

The L-Model

Passivity

Results


PRONTO

Simulation Results

Conclusion

a 0Tv Q

1 0Q

49 of 123


• The singularity of occuring at allows the efficiency curve to

jump from to and to satisfy this passivity condition

• The fact that we reach this singularity in the W-model and our L-

model, and not in the H-model, is due to the non-physical property of

the H-model that thrust and torque reach 0 at the same advance

angle

Introduction

Path Planning

Minimum Energy

Problem Setting

Vehicle Model

Propeller Theory

Propeller Basics


The H-Model

The L-Model

Passivity

Results


PRONTO

Simulation Results

Conclusion

0Q

T Q

50 of 123

Results

• Drag polar: the L-model does exhibit the desired offset

Introduction

Path Planning

Minimum Energy

Problem Setting

Vehicle Model

Propeller Theory

Propeller Basics


The H-Model

The L-Model

Passivity

Results


PRONTO

Simulation Results

Conclusion

H-model

W-model

L-model

( )c L

( )c D

51 of 123

Results


• Lift and drag coefficients: low-order harmonic approximation

Introduction

Path Planning

Minimum Energy

Problem Setting

Vehicle Model

Propeller Theory

Propeller Basics


The H-Model

The L-Model

Passivity

Results


PRONTO

Simulation Results

Conclusion

( )c L

( )c D

52 of 123

Results


• Lift and drag coefficients: low-order harmonic approximation

• Thrust and torque coefficients: much better approximation, not

simultaneously crossing 0

Introduction

Path Planning

Minimum Energy

Problem Setting

Vehicle Model

Propeller Theory

Propeller Basics


The H-Model

The L-Model

Passivity

Results


PRONTO

Simulation Results

Conclusion

10 ( )c Q

( )c T

53 of 123

The Optimization ProblemCost function, collision avoidance, desired trajectories, terminal cost.

Introduction

Path Planning

Minimum Energy

Problem Setting

Vehicle Model

Propeller Theory


PRONTO

Simulation Results

Conclusion

54 of 123

Multiple Vehicles: Notation

• We denote system states and inputs for the vehicles , as

and write the system dynamics in standard notation as

• Similarly, obstacle is referred to as

Introduction

Path Planning

Minimum Energy

Problem Setting

Vehicle Model

Propeller Theory


Notation

Cost Functional

Terminal Conditon

Collision Avoidance

Desired Trajectories

Complete Problem

PRONTO

Simulation Results

Conclusion

[ ] [ ] [ ], ,i i it f t t tx x u

[ ] [ ] [ ] [ ]1 2 3 o o ok k k

k k k k x y r o o o oTT

V1, ,, i i N

[ ] [ ] [ ]1 8 ps sb

[ ] [ ] [ ]1 2 ps sb

i iii i i

i i i i i

i i i

x y u v r

x x x

u u u

TT

O1, ,k N

55 of 123

Cost Functional

• From quasi-steady electrical equations, we obtain the cost functional

as

where the motor voltages and , and currents and depend

on the chosen propeller model and are functions of and

Introduction

Path Planning

Minimum Energy

Problem Setting

Vehicle Model

Propeller Theory


Notation

Cost Functional

Terminal Conditon

Collision Avoidance


Complete Problem

PRONTO

Simulation Results

Conclusion

[ ]iu

[ ] [ ] [ ] [ ] [ ] [ ]ps ps sb sb payloadpow ,i i i i i il t t V t I t V t I t P x u

[ ]psiV [ ]

sbiV

[ ]psiI [ ]

sbiI

[ ]ix

56 of 123

Cost Functional

• For instance, using the L-model, the port side motor‘s power usage is

where and are both functions of and (time dependency and

vehicle index omitted)

Introduction

Path Planning

Minimum Energy

Problem Setting

Vehicle Model

Propeller Theory


Notation

Cost Functional

Terminal Conditon

Collision Avoidance


Complete Problem

PRONTO

Simulation Results

Conclusion

s

psps

p ps

ps ps

ps hyd ps ps hyd

2ps ps p

2ps ps hy

s a p

d

s e ps a ps ps e

2a e2t t

2 2a a ahyd2 2 2

t t t

12

mechanical power requirement

P V I R I K I R I I K

R Kb b

K K

R R Rb b Q b

K K

Q

K

Q

Q

pspshydQ x u

57 of 123

Terminal Condition

• A terminal condition is imposed on each vehicle at final time ,

fixing its pose (coordinates & orientation) and velocities (surge, sway,

and yaw rate)

• Each vehicle has to fulfill the constraint

where the constant vector denotes the terminal condition for

vehicle .

Introduction

Path Planning

Minimum Energy

Problem Setting

Vehicle Model

Propeller Theory


Notation

Cost Functional

Terminal Conditon

Collision Avoidance


Complete Problem

PRONTO

Simulation Results

Conclusion

T

[ ] [ ]f

i iT x x

[ ]fix

i

58 of 123

Collision Avoidance

• Comes in two flavors: inter-vehicle and obstacle collision avoidance

• Except for time dependence, both are similar

• Will be formulated as constraints to the optimization problem

• Can effectively be dealt with by using Euclidean norm as (spatial)

distance measure, resulting in circular obstacles & safety zones

• Different shapes are possible by using different norms

Introduction

Path Planning

Minimum Energy

Problem Setting

Vehicle Model

Propeller Theory


Notation

Cost Functional

Terminal Conditon

Collision Avoidance


Complete Problem

PRONTO

Simulation Results

Conclusion

59 of 123

Collision Avoidance

• Trajectories of two vehicles and are collision-free if and only if

• This defines the inter-vehicle collision avoidance constraint as

Introduction

Path Planning

Minimum Energy

Problem Setting

Vehicle Model

Propeller Theory


Notation

Cost Functional

Terminal Conditon

Collision Avoidance


Complete Problem

PRONTO

Simulation Results

Conclusion

2 2[

co

] [ ] [ ] [ ]1 1 2 2[ ] [ ]

2 2l

c c

, : 12 2

0

i j i j

i jt t t t

c t tr r

x x x x

x x

2 2 2[ ] [ ] [ ] [ ]

1 1 2 2 c 0,2 i j i j tt t t t Tr x x x x

i j

60 of 123

Collision Avoidance

• A trajectory of vehicle and an obstacle are collision-free iff

• This defines the obstacle collision avoidance constraint as

Introduction

Path Planning

Minimum Energy

Problem Setting

Vehicle Model

Propeller Theory


Notation

Cost Functional

Terminal Conditon

Collision Avoidance


Complete Problem

PRONTO

Simulation Results

Conclusion

2 2[ ] [ ] [ ] [ ]1 1 2 2[ ] [ ]

2 2[ ] [ ]c c 3

obs

3

, : 1 0

i k i k

i k

k k

t tc t

r r

x o x ox o

o o

i k

2 2 2[ ] [ ] [ ] [ ] [ ]

1 1 2 2 c 3 0, i k i k kt t r t T x o x o o

61 of 123


• Optionally: track a pre-specified system trajectory

• Using appropriately scaled positive definite matrices and , we

can specify an additional, optional cost term for desired trajectories in

a weighted sense as

Introduction

Path Planning

Minimum Energy

Problem Setting

Vehicle Model

Propeller Theory


Notation

Cost Functional

Terminal Conditon

Collision Avoidance


Complete Problem

PRONTO

Simulation Results

Conclusion

[ ] [ ]des des,i ix u

2L

T T

2 2[ ] [ ] [ ] [ ] [ ] [ ]des des des

1 1, ,

2 2i i i i i i

Q Rl t t t t t t t x u x x u u

TQ TR

62 of 123

The complete problem

• cost functional

• subject to constraints

• dynamics

• collision avoidance

• obstacle avoidance

• with initial condition: e.g., trajectories obtained by projecting the

result of a globally optimal pre-planner onto the trajectory manifold

Introduction

Path Planning

Minimum Energy

Problem Setting

Vehicle Model

Propeller Theory


Notation

Cost Functional

Terminal Conditon

Collision Avoidance


Complete Problem

PRONTO

Simulation Results

Conclusion

[ ] [ ] [ ] [pow des

]

10min , , ,v

T N i i i i

il l d m T

x u x u x

[ ] [ ] [ ] [ ] [ ] [ ] [ ]0 f, , 0i i i i i i if t T x x u x x x x , ,

[ ]col v

[ ] 1, ,0 ,,i j ic jt i j Nt x x , ,

[ ] [ ]o s ob , 0 1, ,i kc t k N x o ,

63 of 123

The Projection Operator ApproachIntroduction to PRONTO and the barrier function.

Introduction

Path Planning

Minimum Energy

PRONTO

Simulation Results

Conclusion

64 of 123

Minimization of trajectory functionals

• Consider the problem of minimizing a functional

over the set of bounded trajectories of the nonlinear system

Here, and .

• We write this constrained problem as , where

- is a bounded curve with continuous

- means and

Introduction

Path Planning

Minimum Energy

PRONTO

Minimization of Trajectory Functionals

Projection Operator Approach

Trajectory Manifold

Equivalence

Newton Method

Derivatives

Summary

Barrier Function

Simulation Results

Conclusion

mu t

0

, , ,T

h x u l x u d m x T

T

0, , 0x t f x t u t t x x ,

nt x

min h T

,

00x T ,t f t u t

[by courtesy of J. Hauser] 65 of 123

How do we solve this?

• We could think of simply using a shooting approach, i.e.,

optimization over if the system is sufficiently stable

• However, this is often “computationally useless”, since small

changes in might lead to large changes in

Introduction

Path Planning

Minimum Energy

PRONTO



Trajectory Manifold

Equivalence

Newton Method

Derivatives

Summary

Barrier Function

Simulation Results

Conclusion

u t

u x


Projection operator approach

Key idea: a trajectory tracking controller may be used to minimize the

effects of system instabilities, providing a numerically effective,

redundant trajectory parameterization.

• Let , be a bounded curve.

• Let , be the trajectory of determined by the

nonlinear feedback system

Introduction

Path Planning

Minimum Energy

PRONTO



Trajectory Manifold

Equivalence

Newton Method

Derivatives

Summary

Barrier Function

Simulation Results

Conclusion

, 0, tt t t

, 0, x u tt t t

0, 0, x f x u x x

u t K t t x

f


Projection operator approach

• Using the nonlinear feedback , the map

is a continuous, nonlinear projection operator, [Hauser 2002]

mapping state-control curves into trajectores of the manifold .

• For each , the curve is a trajectory.

(The trajectory contains both state and control curves.)

Introduction

Path Planning

Minimum Energy

PRONTO



Trajectory Manifold

Equivalence

Newton Method

Derivatives

Summary

Barrier Function

Simulation Results

Conclusion

[Figure by courtesy of A. Saccon]

: , ,ux P

Pdom P

t K t tt tu x

T


Projection Operator Properties

Suppose that is and that is bounded and exponentially stabilizes

. Then

• is well defined on an neighborhood of

• is (Fréchet differentiable wrt. norm)

• for all

• if and only if

• (projection)

Introduction

Path Planning

Minimum Energy

PRONTO



Trajectory Manifold

Equivalence

Newton Method

Derivatives

Summary

Barrier Function

Simulation Results

Conclusion P P P

0

f rC K

0 T

P

P rC

L

L

P T dom P

T P



Projection Operator Properties

• On the finite interval , choose to obtain stability-like

properties so that the modulus of continuity of is relatively small.

• On the infinite horizon, instabilities must be stabilized in order to

obtain a (well-defined) projection operator (e.g., for ).

Introduction

Path Planning

Minimum Energy

PRONTO



Trajectory Manifold

Equivalence

Newton Method

Derivatives

Summary

Barrier Function

Simulation Results

Conclusion

x x u

0,T K

P

0K



Derivatives of P

We may use ODEs to calculate and

The derivatives are about the trajectory

The feedback stabilizes the state at each level

Introduction

Path Planning

Minimum Energy

PRONTO



Trajectory Manifold

Equivalence

Newton Method

Derivatives

Summary

Barrier Function

Simulation Results

Conclusion

21,

2D D PP P P

D P

0: , 0,

x f x u x x

u t K t t x

: 0 0

i i i i i

i i i i

z A t z B t v z

v v t K t t z

21 1 2 2: , 0 0,

y A t y B t w D f t t t y

w K t y

P

21 2,D P

K

1 22

,

,

,

,

,

,

i i i i i i

x u

z D

y D

v D

w

P P

P

P P


Trajectory Manifold

• Theorem: is a Banach manifold. Every near can be

uniquely represented as , with .

• Key: the continuous linear projection operator provides the

required subspace splitting.

Note: if and only if .

• The Representation Theorem provides a (local) linear

parameterization of (nonlinear) trajectories.

Introduction

Path Planning

Minimum Energy

PRONTO



Trajectory Manifold

Equivalence

Newton Method

Derivatives

Summary

Barrier Function

Simulation Results

Conclusion

D P

T T


T T T

P

D P

T T


Equivalent Optimization Problems

• Composing the cost functional with the projection operator to

obtain the unconstrained trajectory functional

(remember that ) for

, we see that

are equivalent in the sense that

a) if is a constrained local minimum of , then it is an

unconstrained local minimum of ; and

b) if is an unconstrained local minimum of in , then

is a constrained local minimum of .

Introduction

Path Planning

Minimum Energy

PRONTO



Trajectory Manifold

Equivalence

Newton Method

Derivatives

Summary

Barrier Function

Simulation Results

Conclusion

g h P

dom U P

0

, , ,T

h x u l x u d m x T

min min

constrained unconstrained

and h g

T U

h

* T U h

g

U Ug

* P


Equivalent Optimization Problems

• This equivalence allows the development of Newton descent

methods for the optimization of over , as every near

can be uniquely represented as , where

(i.e., is a tangent vector).

• At each iteration, we construct and minimize a second order

approximation of around the current trajectory ; the

minimization is restricted to the tangent space.

Introduction

Path Planning

Minimum Energy

PRONTO



Trajectory Manifold

Equivalence

Newton Method

Derivatives

Summary

Barrier Function

Simulation Results

Conclusion

t

h T T

T P T T

ig


PRojection Operator based Newton method for Trajectory Optimization (PRONTO)

Introduction

Path Planning

Minimum Energy

PRONTO



Trajectory Manifold

Equivalence

Newton Method

Derivatives

Summary

Barrier Function

Simulation Results

Conclusion

[Figures by courtesy of A. Saccon]

Trajectory Manifold Descent Direction

Line Search Update

75 of 123


Introduction

Path Planning

Minimum Energy

PRONTO



Trajectory Manifold

Equivalence

Newton Method

Derivatives

Summary

Barrier Function

Simulation Results

Conclusion

0

0,1

1

0,1,2,

1 2arg min ,2

arg min

given initial trajectory

feedback , defining about

i

i i i

i i i

i i i i

T

i

K

Dh D g

h

P

T

P

P

for

design

search direction

line search

update

end



• This direct method generates a descending trajectory sequence in

Banach space, with quadratic convergence to second-order sufficient

minimizers.

• When is not positive definite on , we can obtain a quasi-

Newton descent direction by solving

where is positive definite on (e.g., an approximation to

).

Introduction

Path Planning

Minimum Energy

PRONTO



Trajectory Manifold

Equivalence

Newton Method

Derivatives

Summary

Barrier Function

Simulation Results

Conclusion

iTT

1

arg min ,2i

i i iT

Dh q

T

2iD g

2iD g

iq i

TT


Derivatives

• The first and second Fréchet derivatives of

are given by

• When and , they specialize to

(remember that iff. )

Introduction

Path Planning

Minimum Energy

PRONTO



Trajectory Manifold

Equivalence

Newton Method

Derivatives

Summary

Barrier Function

Simulation Results

Conclusion

2 21

222 21 1, ,,

Dh D

D g D D Dh D

Dg

D h

P P

P P P P P

i T T

2 21 21 1 2

22 , ,,

generalizes Lagrange multiplier

Dg Dh

D g Dh hD D

P

DP

g h P

T

T T


D2g Lagrange multiplier

where

(remember that

with )

We obtain a stabilized adjoint variable, independent of stationary

considerations!

(For nonzero terminal cost, .)

Introduction

Path Planning

Minimum Energy

PRONTO



Trajectory Manifold

Equivalence

Newton Method

Derivatives

Summary

Barrier Function

Simulation Results

Conclusion

2 21

0

, , ,T

D l DDh D d P P

21 1 2 2: ,y A t y B t w D f t t t

21

0 0

21

0

2

0

,

,

,

, ,

, ,T

c

T T

c

s

T

ID l s D f s s s dsd

K

ID l s d D f s s s ds

K

q s D f s s s ds

T

0, qx ut A B K t q t l t K t t Tq t lt T

T T T

xq T m x T T

0 0w K t y y ,


D2g

• For and , has the form

where

has the elements

and .

• In fact, is the second derivative matrix of the Hamiltonian

Again, no stationary considerations.

Introduction

Path Planning

Minimum Energy

PRONTO



Trajectory Manifold

Equivalence

Newton Method

Derivatives

Summary

Barrier Function

Simulation Results

Conclusion

2 ,D g T T T

1

0

T z Q S zd z T Pz T

v S R v

T

T

T

Q SW t

S R

T

22

1

,i j i j

nk

ij kk

flw t t q ttt

2

1 2

mP x T

x

W

, , , , , ,H x u q t l x u t q f x u T


Descent Direction LQ Optimal Ctrl. Problem

• The descent direction problem is a linear quadratic optimal control

problem

where the cost is, in general, non-convex.

• This linear quadratic optimal control problem (with positive definite

) has a unique solution if and only if

has a bounded solution on .

(remember that and )

Introduction

Path Planning

Minimum Energy

PRONTO



Trajectory Manifold

Equivalence

Newton Method

Derivatives

Summary

Barrier Function

Simulation Results

Conclusion

1 1

0

1 1mi

0 0

n2 2

s. t . , z

T a z z Q S zd r z T z T Pz T

b v v

A t z B t v

S R v

z

T

T

T

R

0,T

110, P PA PBR BA P P T PP Q T T

1 TA A BR S 1 TQ Q SR S


In other words, …

Introduction

Path Planning

Minimum Energy

PRONTO



Trajectory Manifold

Equivalence

Newton Method

Derivatives

Summary

Barrier Function

Simulation Results

Conclusion

A trajectory tracking controller defines a function space operator that maps a

desired trajectory (a curve) to a system trajectory (element of trajectory

manifold)

Composing the optimization objective (a functional) with the (trajectory

tracking) projection operator converts a dynamically constrained OCP into

unconstrained problem (functional)

This functional can be expanded as Taylor series

Making use of the representation theorem (Riesz), we know that the proj.

operator gives an one-to-one and onto-mapping between tangent space and

traj. manifold

Then we can search the quadratic polynomial (composition of cost functional

and projection operator) on the tangent space

Now determine whether there is a descent direction (to 2nd or 1st order)

Again with the representation theorem, we can do the line search (looking at

the value of h(P(ξi+γiζi)), which gives the tool for choosing a step size that

will result in sufficient decrease)

82 of 123

The βδ barrier function

• Key difficulty with standard log barrier methods: infeasibility of is

not tolerated

• Therefore: not possible to evaluate the cost functional unless is a

feasible curve

• Even if is feasible, we cannot be sure that is

Introduction

Path Planning

Minimum Energy

PRONTO



Trajectory Manifold

Equivalence

Newton Method

Derivatives

Summary

Barrier Function

βδ barrier

Hockey Stick Extension

Simulation Results

Conclusion

P



• Solution: define, for , the approximate log barrier function

as [Hauser and Saccon, 2006]

• Use the approximate barrier functional

to add the constraints

• Note: can be evaluated on any curve in .

Introduction

Path Planning

Minimum Energy

PRONTO



Trajectory Manifold

Equivalence

Newton Method

Derivatives

Summary

Barrier Function

βδ barrier


Simulation Results

Conclusion

211

2

log

2log

z z

z zz

0 1

: , 0,

0

, ,j

T

jb c d

minh b

T

T h b



• retains many of the important properties of the standard log

barrier function while expanding the domain of finite

values from to

• Now: use the projection operator based Newton method to optimize

the functional

as part of a continuation method

Introduction

Path Planning

Minimum Energy

PRONTO



Trajectory Manifold

Equivalence

Newton Method

Derivatives

Summary

Barrier Function

βδ barrier


Simulation Results

Conclusion

z

0,

log z z

,

,g h P P


The “Hockey Stick” Extension

• Problem: assumes (unbounded) negative values for ,

putting an undesired reward into collision avoidance constraint

(In collision avoidance, being far away is not better than being merely

feasible!)

• Solution: extend the barrier functional by forming a composition

with the smooth “hockey stick”

Introduction

Path Planning

Minimum Energy

PRONTO



Trajectory Manifold

Equivalence

Newton Method

Derivatives

Summary

Barrier Function

βδ barrier


Simulation Results

Conclusion

1z z

n 0ta h

otherwise

z zz

z

2

C

86 of 123

βδ barrier with hockey stick

Introduction

Path Planning

Minimum Energy

PRONTO



Trajectory Manifold

Equivalence

Newton Method

Derivatives

Summary

Barrier Function

βδ barrier


Simulation Results

Conclusion

βδ for different values of δ βδ ◦ σ for different δ

87 of 123

Simulation ResultsBathymetry-based planning, survey scenarios, and obstacle avoidance.

Introduction

Path Planning

Minimum Energy

PRONTO

Simulation Results

Conclusion

88 of 123

Putting things into action

• On the implementation side,

• using an existing toolkit version, PRONTO was implemented as a

cooperative motion planner for Matlab, with the core files all

written in C, and extended with constraints;

• an automation mechanism was conceived that uses XML to

distribute various parameters into C headers and M files;

• bathymetric maps can automatically be transformed from point

clouds to C header files; and

• modularity allows to interrupt execution at any point to invoke

scripts that ease understanding.

Introduction

Path Planning

Minimum Energy

PRONTO

Simulation Results

Implementing the Cooperative Motion Planning Problem

Bathymetry-based Trajectory Planning

Survey Scenarios

Obstacle Avoidance

Conclusion

89 of 123

Putting things into action

• Using the toolkit requires analytic computation of first and second

derivatives of all model, thruster, and motor equations, and all

constraint functions.

• To gain insight into the processes, this was done not only for

MEDUSAS, but also a ground robot, various other flavors of MEDUSAS,

an additionally developed propeller model based on momentum

theory, and single thrusters submersed in open water.

Introduction

Path Planning

Minimum Energy

PRONTO

Simulation Results



Survey Scenarios

Obstacle Avoidance

Conclusion

90 of 123

Terrain-based trajectory planning

• Trajectory tracking, and generally, navigation needs self-localization

along a planned trajectory

• For ground and aerial robots, localization data is available as GPS

signals

• These GPS signals are not available in the underwater realm

(reflection at the sea surface)

Introduction

Path Planning

Minimum Energy

PRONTO

Simulation Results



Survey Scenarios

Obstacle Avoidance

Conclusion

91 of 123


• Solution: use existing maps (e.g., geology of the sea bottom or earth‘s

magnetic field) for self-localization based on on-board measurements

(e.g., sonar or magnetometer)

• The quality of self-localization is proportional on richness of features

in the sensor inputs

• Including the objective of going over feature-rich terrain already at

the planning stage therefore makes accurate trajectory tracking easier

Introduction

Path Planning

Minimum Energy

PRONTO

Simulation Results



Survey Scenarios

Obstacle Avoidance

Conclusion

92 of 123


• Terrain features may be incorporated in two ways:

1. a pre-planner uses a discretized map of the environment to

generate a globally optimal set of paths (one for each vehicle)

between initial and final poses that, projected onto the trajectory

manifold, form a set of desired trajectories

2. an additional term is added to the integral cost functional; the

(neccessarily) discrete map is interpolated in a C2 smooth manner

(e.g., using a bi-cubic approximation) to form a (possibly

weighted) continuous cost

Introduction

Path Planning

Minimum Energy

PRONTO

Simulation Results



Survey Scenarios

Obstacle Avoidance

Conclusion

93 of 123


• Pre-planning turns out to be far more effective, since it does not add

to the complexity of the planning problem (no additional term that

needs to be weighted off against other competing factors as energy

usage, or barrier constraint terms)

• In fact, a continuous terrain cost should only have a marginal impact

so as not to affect “more important“ objectives

Introduction

Path Planning

Minimum Energy

PRONTO

Simulation Results



Survey Scenarios

Obstacle Avoidance

Conclusion

94 of 123

Introduction

Path Planning

Minimum Energy

PRONTO

Simulation Results



Survey Scenarios

Obstacle Avoidance

Conclusion

This is a 443-by-843 meter of the D. João de Castro seamount in the Azores region of the Atlantic Ocean. Map resolution is 1 meter.

Seafloor Map MC(x,y)

95 of 123

Introduction

Path Planning

Minimum Energy

PRONTO

Simulation Results



Survey Scenarios

Obstacle Avoidance

Conclusion

Generated from the seafloor map using a Fisher information matrix based measure (i.e., related to the norm of the terrain gradient. The resolution was downscaled to 10-by-10 meter squares.

Information Map MI(x,y)

96 of 123

Introduction

Path Planning

Minimum Energy

PRONTO

Simulation Results



Survey Scenarios

Obstacle Avoidance

Conclusion

A normalized version of the information map generated in the previous step.

Excitation Map ME(x,y)

97 of 123

This could simply be the inversion of the normalized excitation map.

Here, a nonlinear cost is used and mapped

from into to ensure that the Euclidean distance is an

admissible and consistent heuristic for A*.

Introduction

Path Planning

Minimum Energy

PRONTO

Simulation Results



Survey Scenarios

Obstacle Avoidance

Conclusion

Cost Map MC(x,y)

10,20

C E2cos ,M M x y

0,1

98 of 123

Introduction

Path Planning

Minimum Energy

PRONTO

Simulation Results



Survey Scenarios

Obstacle Avoidance

Conclusion

Before being useful to the optimizer, the seafloor map needs to

a) have its gradient information extracted, which then is

b) normalized and

c) treated in an appropriate manner to form a cost map.

Map Preprocessing

99 of 123

Resulting paths and trajectories

Introduction

Path Planning

Minimum Energy

PRONTO

Simulation Results



Survey Scenarios

Obstacle Avoidance

Conclusion

Result of A*

(The desired trajectories)

Projected paths

(The initial condition)100 of 123

Bathymetry-based trajectories

Introduction

Path Planning

Minimum Energy

PRONTO

Simulation Results



Survey Scenarios

Obstacle Avoidance

Conclusion

101 of 123

Braid maneuver vs. lawn mower

• Survey scenarios aim for maximum ground coverage (usually at

constant altitude)

• The “lawn mower” pattern is the approach, both in single and

multiple vehicle missions

Introduction

Path Planning

Minimum Energy

PRONTO

Simulation Results



Survey Scenarios

Obstacle Avoidance

Conclusion

102 of 123


• However, it exhibits disadvantages:

• along the straight line segments of the lawn mower, (single)

beacon navigation is impossible: it was shown that observability

(and thus, quality of navigation) can only be assured for non-

straight line paths [Bayat et al. 2012]

• the paths are not differentiable, resulting in jerky motor inputs at

transitions from straight lines to arcs and back

Introduction

Path Planning

Minimum Energy

PRONTO

Simulation Results



Survey Scenarios

Obstacle Avoidance

Conclusion

103 of 123


• The “braid maneuver” was developed to overcome these drawbacks:

• no straight lines

• trajectories are at least C2, i.e., high motor transients are avoided

• in addition, the smooth motion can be executed at (almost)

constant speed

• Single remaining advantage of the lawn mower pattern over the braid

maneuver: the lawn mower is compatible with maintaining a rigid

group formation

Introduction

Path Planning

Minimum Energy

PRONTO

Simulation Results



Survey Scenarios

Obstacle Avoidance

Conclusion

104 of 123

Generating a “good” initial condition

• Coordination space pre-planning may be used to generate an initial

condition that represents a heuristic on how to satisfy inter-vehicle

and obstacle collision avoidance constraints during planning

• This pre-planning step is performed on an dimensional discrete

space whose dimensions represent spatial or temporal coordinates of

the trajectories in question [LaValle 2006]

• Original set of paths is a (hyper-)line that crosses this “coordination

space” diagonally

• Collisions among pairs of vehicles (either in space or in time) are

represented as (hyper-) cylinders in the coordination space

Introduction

Path Planning

Minimum Energy

PRONTO

Simulation Results



Survey Scenarios

Obstacle Avoidance

Conclusion

VN

105 of 123


• With a discrete planner (e.g., A*), a path can be found through the

coordination space, connecting the two diagonal extrema without

intersecting any of the cylinders

• The result is a new trajectory traversal law ensuring that the vehicles

are coordinated in such a manner along their previously conflicting

trajectories that collisions are avoided

Introduction

Path Planning

Minimum Energy

PRONTO

Simulation Results



Survey Scenarios

Obstacle Avoidance

Conclusion

106 of 123


• Important: the spatial coordinates are not changed through this!

• Important: the resulting curves need to be projected onto the

trajectory manifold once more to ensure feasibility in vehicle

dynamics!

• Important: this is a heuristic, and as such does not guarantee global

optimality of the planning result!

Introduction

Path Planning

Minimum Energy

PRONTO

Simulation Results



Survey Scenarios

Obstacle Avoidance

Conclusion

107 of 123

Braid maneuver

Introduction

Path Planning

Minimum Energy

PRONTO

Simulation Results



Survey Scenarios

Obstacle Avoidance

Conclusion

108 of 123

Randomized Obstacle Field

• Static obstacles at sea: sea mounts, oil rigs and other man-made

structures, research vessels, …

• For simplicity of mathematics: circular, convex obstacle shapes (easy

to adapt to other obstacle shapes by overlapping)

Introduction

Path Planning

Minimum Energy

PRONTO

Simulation Results



Survey Scenarios

Obstacle Avoidance

Conclusion

109 of 123

Randomized Obstacle Field

• Test scenario: randomly generated obstacle field with uniform

distribution in the interval [0m, 50m] (local coordinates) for obstacle

positions and in the interval [1m, 3m] in their radii

• Initial curves (and desired trajectories) deliberately chosen so that

they intersect obstacles

• To make things even tougher, we also go for a formation change

Introduction

Path Planning

Minimum Energy

PRONTO

Simulation Results



Survey Scenarios

Obstacle Avoidance

Conclusion

110 of 123

Obstacle field + formation change

Introduction

Path Planning

Minimum Energy

PRONTO

Simulation Results



Survey Scenarios

Obstacle Avoidance

Conclusion

111 of 123

ConclusionRecap & Outlook.

Introduction

Path Planning

Minimum Energy

PRONTO

Simulation Results

Conclusion

112 of 123

Recap

• Minimum energy motion planning algorithm with

• explicitly incorporated vehicle dynamics;

• coordination space pre-planner;

• first-order thruster dynamics; and

• terrain-based pre-planning

Introduction

Path Planning

Minimum Energy

PRONTO

Simulation Results

Conclusion

Recap

Outlook

References

113 of 123

Recap

• New four-quadrant propeller model that

• is conform with basic propeller theory;

• preserves the key physical properties of the original four-

quadrant model; and

• overcomes some of the difficulties of a well-known simplification

with only three additional parameters (five in total)

Introduction

Path Planning

Minimum Energy

PRONTO

Simulation Results

Conclusion

Recap

Outlook

References

114 of 123

Outlook

• Future work is aimed towards

• trajectory generation and optimization for rigid formations of

vehicles;

• bi-cubic interpolation of terrain and integration into PRONTO

itself;

Introduction

Path Planning

Minimum Energy

PRONTO

Simulation Results

Conclusion

Recap

Outlook

References

115 of 123

Outlook

• include communication between vehicles to allow for per-vehicle

trade-off in terrain information to the benefit of the group so that

terrain information is maximized over the whole formation, or to

allow for some vehicles to complement dead reckoning with

single beacon navigation; and

• extend coordination space to static obstacles and “circumvention

decisions”.

Introduction

Path Planning

Minimum Energy

PRONTO

Simulation Results

Conclusion

Recap

Outlook

References

116 of 123

Outlook

• Possible avenues for PRONTO are

• replacing the Armijo rule with Nesterov‘s method; and

• in view of multiple vehicle applications, investigate into

distributed Newton methods.

Introduction

Path Planning

Minimum Energy

PRONTO

Simulation Results

Conclusion

Recap

Outlook

References

117 of 123

Thank you!

Research supported in part by project MORPH of the EU FP7 (grant agreement no. 288704, andby the FCT Program PEst-OE/EEI/LA0009/2011.

The work of A. Häusler was supported by a Ph.D. scholarship of the FCT under grant numberSFRH/BD/68941/2010.

Introduction

Path Planning

Minimum Energy

PRONTO

Simulation Results

Conclusion

Recap

Outlook

References

118 of 123

References

A. Related to this work

• Häusler, A. J., Saccon, A., Hauser, J., Pascoal, A. M., and Aguiar, A. P. (2014)

Energy-Optimal Motion Planning for Multiple Vehicles with Collision Avoidance.

IEEE Transactions on Control Systems Technology, submitted.

• Häusler, A. J., Saccon, A., Hauser, J., Pascoal, A. M., and Aguiar, A. P. (2013) Four-

Quadrant Propeller Modeling. A Low-Order Harmonic Approximation in

Proceedings of the 9th IFAC Conference on Control Applications in Marine Systems

(CAMS).

• Häusler, A. J., Saccon, A., Pascoal, A. M., Hauser, J., and Aguiar, A. P. (2013)

Cooperative AUV Motion Planning using Terrain Information in Proceedings of the

OCEANS '13 MTS/IEEE Bergen.

• Häusler, A. J., Saccon, A., Aguiar, A. P., Hauser, J., and Pascoal, A. M. (2012)

Cooperative Motion Planning for Multiple Autonomous Marine Vehicles in

Proceedings of the 9th IFAC Conference on Manoeuvring and Control of Marine Craft

(MCMC 2012).

Introduction

Path Planning

Minimum Energy

PRONTO

Simulation Results

Conclusion

Recap

Outlook

References

119 of 123

References

B. Cited in the presentation

• Bayat, M. and Aguiar, A. P. (2012) Observability Analysis for AUV Range-Only

Localization and Mapping. Measures of Unobservability and Experimental Results

in Proceedings of the 9th IFAC Conference on Manoeuvring and Control of Marine Craft

(MCMC 2012).

• Bryson, A. E. and Ho, Y.-C. (1975) Applied Optimal Control. Optimization, Estimation,

and Control, Taylor & Francis, Levittown, PA.

• Francis, S. L. X., Anavatti, S. G., and Garratt, M. (2013) Real-Time Cooperative Path

Planning for Multi-Autonomous Vehicles in International Conference on Advances in

Computing, Communications and Informatics (ICACCI).

• Ghabcheloo, R., Aguiar, A. P., Pascoal, A. M., Silvestre, C., Kaminer, I. I., and

Hespanha, J. P. (2009) Coordinated Path-Following in the Presence of

Communication Losses and Time Delays. SIAM Journal of Control and Optimization,

48(1).

• Yakimenko, O. (2000) Direct Method for Rapid Prototyping of Near-Optimal

Aircraft Trajectories. AIAA Journal of Guidance, Control, and Dynamics, 23(5).

Introduction

Path Planning

Minimum Energy

PRONTO

Simulation Results

Conclusion

Recap

Outlook

References

120 of 123

References

B. Cited in the presentation (cont‘d)

• Hauser, J. (2002) A Projection Operator Approach to the Optimization of Trajectory

Functionals in Proceedings of the 15th IFAC World Congress.

• Hauser, J. and Saccon, A. (2006) A Barrier Function Method for the Optimization of

Trajectory Functionals with Constraints in Proceedings of the 45th IEEE Conference on

Decision and Control (CDC), pp. 864--869.

• Healey, A. J., Rock, S. M., Cody, S., Miles, D., and Brown, J. P. (1994) Toward an

Improved Understanding of Thruster Dynamics for Underwater Vehicles in

Symposium on Autonomous Underwater Vehicle Technology.

• Ioannidis, K., Sirakoulis, G. C., and Andreadis, I. Depicting Pathways for

Cooperative Miniature Robots using Cellular Automata.

Introduction

Path Planning

Minimum Energy

PRONTO

Simulation Results

Conclusion

Recap

Outlook

References

121 of 123

References


• Kuwata, Y. and How, J. P. (2011) Cooperative Distributed Robust Trajectory

Optimization Using Receding Horizon MILP. IEEE Transactions on Control Systems

Technology, 19(2).

• LaValle, S. M. (2006) Planning Algorithms, Cambridge University Press, Cambridge,

New York.

• Lee, J.-W. and Kim, H. J. (2007) Trajectory Generation for Rendezvous of

Unmanned Aerial Vehicles with Kinematic Constraints in Proceedings of the IEEE

International Conference on Robotics and Automation (ICRA).

• Lewis, L. R., Ross, I. M., and Gong, Q. (2007) Pseudospectral Motion Planning

Techniques for Autonomous Obstacle Avoidance in Proceedings of the 65th IEEE

Vehicular Technology Conference.

Introduction

Path Planning

Minimum Energy

PRONTO

Simulation Results

Conclusion

Recap

Outlook

References

122 of 123

References


• Lian, F.-L. (2008) Cooperative Path Planning of Dynamical Multi-Agent Systems

using Differential Flatness Approach. International Journal of Control, Automation,

and Systems, 6(3), 401.

• Oosterveld, M. W. C. (1970) Wake Adapted Ducted Propellers, Wageningen, The

Netherlands.

• Saccon, A., Aguiar, A. P., Häusler, A. J., Hauser, J., and Pascoal, A. M. (2012)

Constrained Motion Planning for Multiple Vehicles on SE(3) in Proceedings of the

51st Conference on Decision and Control.

• van Lammeren, W. P. A., van Manen, J. D., and Oosterveld, M. W. C. (1969) The

Wageningen B-Screw Series. Transactions of SNAME, 77.

• Yakimenko, O. A. (2000) Direct Method for Rapid Prototyping of Near-Optimal

Aircraft Trajectories. AIAA Journal of Guidance, Control, and Dynamics, 23(5).

Introduction

Path Planning

Minimum Energy

PRONTO

Simulation Results

Conclusion

Recap

Outlook

References

123 of 123

Multiple Vehicle Motion Planning: An Infinite Diminsion Newton Optimization Method

Science

Transcript of Multiple Vehicle Motion Planning: An Infinite Diminsion Newton Optimization Method