In this invited talk at the LARSyS Summer School 2014, we describe a numerical algorithm for multiple vehicle motion planning that addresses explicitly temporal and spatial specifications, as well as energy-related constraints. As a motivating example, we cite the case where a group of vehicles is tasked to reach a number of target points at the same time (simultaneous arrival problem) and avoid inter-vehicle as well as vehicle/obstacle collision, subject to the constraint that the overall energy required for vehicle motion be minimized.
The methodology adopted builds on a numerical method for solving optimal control problems that is known as the PRojection Operator based Newton method for Trajectory Optimization (PRONTO)—a method that avoids the transcription phase typical in direct methods for numerical optimal control and that employs an infinite dimension Newton method to achieve second order convergence of the trajectory optimization problem.
With the theoretical set-up adopted, the vehicle dynamics are taken explicitly into account at the planning level. Thus, in contrast to some of the planning methods available in the literature, the method proposed allows for the direct incorporation of dynamical constraints imposed by the physical characteristics of the vehicles, motion actuators, and even energy sources (e.g. batteries). Should the problem to be solved be feasible, the method yields energy-optimal trajectories without the need to separate the steps of path planning and trajectory generation, as is customary in many of the motion planning methods described in the literature. Restrictive system properties such as differential flatness are not required.
Physics Serway Jewett 6th edition for Scientists and Engineers
Multiple Vehicle Motion Planning: An Infinite Diminsion Newton Optimization Method
1. LARSyS Summer School, July 10, 2014
Multiple Vehicle
Motion Planning
An Infinite Dimension Newton Optimization Method
Andreas J. Häusler
Laboratory of Robotics and Systems in Science and Engineering
Instituto Superior Técnico
2. 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
5. 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
Go-To-
Formation
Maneuver
Literature
Contribution
Path Planning
Minimum
Energy
PRONTO
Simulation
Results
Conclusion
5 of 123
6. 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
Go-To-
Formation
Maneuver
Literature
Contribution
Path Planning
Minimum
Energy
PRONTO
Simulation
Results
Conclusion
6 of 123
8. Path Planning Foundations
Introduction
Path Planning
Foundations
Single Vehicle
Multiple
Vehicles
Deconfliction
TCPF
Simulation
Results
Summary
Minimum
Energy
PRONTO
Simulation
Results
Conclusion
0 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
9. 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
10. 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
minv maxv 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
11. Brachistochrone problem
Introduction
Path Planning
Foundations
Single Vehicle
Multiple
Vehicles
Deconfliction
TCPF
Simulation
Results
Spatial and
Temporal
Deconfliction
Ocean
Currents and
Brachisto-
chrone
Problem
Summary
Minimum
Energy
PRONTO
Simulation
Results
Conclusion
Solution of the classical Brachistochrone problem [A. Bryson & Y.-C. Ho 1975]. Initial and final
heading were part of the design variables. (Nearly) constant velocity was achieved by
using the non-polynomial form of η(τ).
11 of 123
12. 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
13. The Minimum Energy
Problem
Planning framework and vehicle modeling.
Introduction
Path Planning
Minimum
Energy
PRONTO
Simulation
Results
Conclusion
13 of 123
14. 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
15. 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
Classical
Energy
Computation
Main
Features
Planning
Framework
Vehicle
Model
Propeller
Theory
Optimization
Problem
PRONTO
Simulation
Results
Conclusion
Thrust T
Ball in
water
Speed v
Drag D
T D
3
P Tv kv
3
E kv dt
2
D kv
15 of 123
16. Classical Energy Computation
II. Motionless in the presence of current
• Now,
• However, the energy spent clearly has to be !
Introduction
Path Planning
Minimum
Energy
Problem
Setting
Classical
Energy
Computation
Main
Features
Planning
Framework
Vehicle
Model
Propeller
Theory
Optimization
Problem
PRONTO
Simulation
Results
Conclusion
0Tv
0E
Thrust T
Ball in
water
Speed v
Drag D
Current vc
v=0
D=0
16 of 123
17. Classical Energy Computation
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
Classical
Energy
Computation
Main
Features
Planning
Framework
Vehicle
Model
Propeller
Theory
Optimization
Problem
PRONTO
Simulation
Results
Conclusion
Thrust T
Ball in
water
Speed v
0
d
a v
dt
3
E kv mav dt
mav
17 of 123
18. 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
Classical
Energy
Computation
Main
Features
Planning
Framework
Vehicle
Model
Propeller
Theory
Optimization
Problem
PRONTO
Simulation
Results
Conclusion
Thrust TBattery Pack D/C Motor Propeller
V I
E P t dt
18 of 123
19. 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
Classical
Energy
Computation
Main
Features
Planning
Framework
Vehicle
Model
Propeller
Theory
Optimization
Problem
PRONTO
Simulation
Results
Conclusion
19 of 123
21. 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
Classical
Energy
Computation
Main
Features
Planning
Framework
Vehicle
Model
Propeller
Theory
Optimization
Problem
PRONTO
Simulation
Results
Conclusion
21 of 123
22. Vehicle Modeling
The MEDUSAS, its motors, and its thrusters.
Introduction
Path Planning
Minimum
Energy
Problem
Setting
Vehicle
Model
Propeller
Theory
Optimization
Problem
PRONTO
Simulation
Results
Conclusion
22 of 123
23. The MEDUSAS
Semi-Submersible
Propulsion 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
Optimization
Problem
PRONTO
Simulation
Results
Conclusion
23 of 123
24. 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
Optimization
Problem
PRONTO
Simulation
Results
Conclusion
24 of 123
25. 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
Optimization
Problem
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
26. 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
Optimization
Problem
PRONTO
Simulation
Results
Conclusion
a e
m t a hyd
a
J b
d
L I R I V K
d
K
t
I Q
a 0d
dtL I
a hyd
t
1
I b Q
K
a 500μHL
a eV R I K
26 of 123
27. 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
Optimization
Problem
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
28. 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
Optimization
Problem
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 2
T a p
2 2
Q a p
2
2
1
2
1
2
T c v v
Q c v v
R
R d
sb
28 of 123
29. Propeller Modeling
The classical open-water model, the four-quadrant
model, and its improvement.
Introduction
Path Planning
Minimum
Energy
Problem
Setting
Vehicle
Model
Propeller
Theory
Optimization
Problem
PRONTO
Simulation
Results
Conclusion
29 of 123
30. 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
Optimization
Problem
PRONTO
Simulation
Results
Conclusion
30 of 123
31. 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
Optimization
Problem
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
32. 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 Four
Quadrants of
Operation
The H-Model
The L-Model
Passivity
Results
Optimization
Problem
PRONTO
Simulation
Results
Conclusion
vp
vtot
va
T
L
D
/ 0.7Q R
32 of 123
33. 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 Four
Quadrants of
Operation
The H-Model
The L-Model
Passivity
Results
Optimization
Problem
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
vp
vtot
va
vp
vtot
va
vp
va
vtot
vp
vtot
va
33 of 123
34. 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 advance angle defines four quadrants of propeller 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 Four
Quadrants of
Operation
The H-Model
The L-Model
Passivity
Results
Optimization
Problem
PRONTO
Simulation
Results
Conclusion
ahead
crash-back
back
crash-ahead
5
24
2
2
2
2
( )
( )
T o
Q o
a
o
d k J
d k
T
Q
J
J
v
d
vp
vtot
va
34 of 123
35. The four-quadrant propeller model
• The advance angle defines four quadrants of propeller operation
• 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 Four
Quadrants of
Operation
The H-Model
The L-Model
Passivity
Results
Optimization
Problem
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
vp
vtot
va
35 of 123
36. 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 Four
Quadrants of
Operation
The H-Model
The L-Model
Passivity
Results
Optimization
Problem
PRONTO
Simulation
Results
Conclusion
0
0
cos ( )sin
cos ( )
( )
( ) sin
T T
Q Q
T
Q
m
c c
k
m
c c
k
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
37. 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 Four
Quadrants of
Operation
The H-Model
The L-Model
Passivity
Results
Optimization
Problem
PRONTO
Simulation
Results
Conclusion
2
37 of 123
38. vp
vtot
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 Four
Quadrants of
Operation
The H-Model
The L-Model
Passivity
Results
Optimization
Problem
PRONTO
Simulation
Results
Conclusion
( ) sin2
( (1) cos2 ) / 2
max
L L
max
D D
c c
cc
H
H
cos sin
sin co / (0.7Rs )
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
39. 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 Four
Quadrants of
Operation
The H-Model
The L-Model
Passivity
Results
Optimization
Problem
PRONTO
Simulation
Results
Conclusion
39 of 123
40. 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
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 Four
Quadrants of
Operation
The H-Model
The L-Model
Passivity
Results
Optimization
Problem
PRONTO
Simulation
Results
Conclusion
40 of 123
41. 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
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 Four
Quadrants of
Operation
The H-Model
The L-Model
Passivity
Results
Optimization
Problem
PRONTO
Simulation
Results
Conclusion
H-model W-model
( )c L
( )c D
41 of 123
42. • The H-model may fail to capture physical constraints:
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 Four
Quadrants of
Operation
The H-Model
The L-Model
Passivity
Results
Optimization
Problem
PRONTO
Simulation
Results
Conclusion
crash-ahead
crash-back
back
ahead
a
2
o
v
d
J
aTv
Q
H-modelW-model
a
2
o
v
d
J
a 0v 0T
0Q
0Q
0 0since and simultaneously, and because
of L'Hôpital's rule, the -m finite everywhodel is ere
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
43. 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 Four
Quadrants of
Operation
The H-Model
The L-Model
Passivity
Results
Optimization
Problem
PRONTO
Simulation
Results
Conclusion
min
( ) sin2( )
( ) 1 cos2( ) / 2
max
L L
max min
D D D D
L
Dc
c o
c
c
c o c
L
L
( )T
( )Tc
0 50 °
( )Qc
vp
vtot
va
43 of 123
44. Analysis
1. Open-water efficiency shows expected behavior
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
Optimization
Problem
PRONTO
Simulation
Results
Conclusion
H-model L-modelW-model
back
ahead
Jo Jo Jo
( )J o
crash-
ahead
crash-
back
44 of 123
45. Analysis
1. Open-water efficiency shows expected behavior
2. Produced thrust is a monotone curve
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
Optimization
Problem
PRONTO
Simulation
Results
Conclusion
( )T
T
Q
n n n
H-model L-modelW-model
( )J o
45 of 123
46. Analysis
1. Open-water efficiency shows expected behavior
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 Four
Quadrants of
Operation
The H-Model
The L-Model
Passivity
Results
Optimization
Problem
PRONTO
Simulation
Results
Conclusion
ideal efficiency (momentum theory)
Wageningen series propeller model
Healey et al.’s approximation
L-model
scaled data from MARIUS vehicle
2
/k JT o
( )T
( )J o
46 of 123
47. 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 Four
Quadrants of
Operation
The H-Model
The L-Model
Passivity
Results
Optimization
Problem
PRONTO
Simulation
Results
Conclusion
oJ
crash-ahead
crash-back
back ahead
> 0,
windmilling
< 0,
windmilling
Tv
Q
a
Jo
47 of 123
48. The propeller as power converter
• 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 Four
Quadrants of
Operation
The H-Model
The L-Model
Passivity
Results
Optimization
Problem
PRONTO
Simulation
Results
Conclusion
oJ
48 of 123
49. The propeller as power converter
• 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 Four
Quadrants of
Operation
The H-Model
The L-Model
Passivity
Results
Optimization
Problem
PRONTO
Simulation
Results
Conclusion
a 0Tv Q
1 0Q
49 of 123
50. The propeller as power converter
• 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 Four
Quadrants of
Operation
The H-Model
The L-Model
Passivity
Results
Optimization
Problem
PRONTO
Simulation
Results
Conclusion
0Q
T Q
50 of 123
51. 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 Four
Quadrants of
Operation
The H-Model
The L-Model
Passivity
Results
Optimization
Problem
PRONTO
Simulation
Results
Conclusion
H-model
W-model
L-model
( )c L
( )c D
51 of 123
52. Results
• Drag polar: the L-model does exhibit the desired offset
• Lift and drag coefficients: low-order harmonic approximation
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
Optimization
Problem
PRONTO
Simulation
Results
Conclusion
( )c L
( )c D
52 of 123
53. Results
• Drag polar: the L-model does exhibit the desired offset
• 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 Four
Quadrants of
Operation
The H-Model
The L-Model
Passivity
Results
Optimization
Problem
PRONTO
Simulation
Results
Conclusion
10 ( )c Q
( )c T
53 of 123
54. The Optimization
Problem
Cost function, collision avoidance, desired trajectories,
terminal cost.
Introduction
Path Planning
Minimum
Energy
Problem
Setting
Vehicle
Model
Propeller
Theory
Optimization
Problem
PRONTO
Simulation
Results
Conclusion
54 of 123
55. 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
Optimization
Problem
Notation
Cost
Functional
Terminal
Conditon
Collision
Avoidance
Desired
Trajectories
Complete
Problem
PRONTO
Simulation
Results
Conclusion
[ ] [ ] [ ]
, ,i i i
t f t t tx x u
[ ] [ ] [ ] [ ]
1 2 3 o o ok k k
k k k k
x y r o o o o
TT
V1, ,,i i N
[ ] [ ] [ ]
1 8 ps sb
[ ] [ ] [ ]
1 2 ps sb
i ii
i 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
56. 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
Optimization
Problem
Notation
Cost
Functional
Terminal
Conditon
Collision
Avoidance
Desired
Trajectories
Complete
Problem
PRONTO
Simulation
Results
Conclusion
[ ]i
u
[ ] [ ] [ ] [ ] [ ] [ ]
ps ps sb sb payloadpow ,i i i i i i
l t t V t I t V t I t P x u
[ ]
ps
i
V [ ]
sb
i
V
[ ]
ps
i
I [ ]
sb
i
I
[ ]i
x
56 of 123
57. 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
Optimization
Problem
Notation
Cost
Functional
Terminal
Conditon
Collision
Avoidance
Desired
Trajectories
Complete
Problem
PRONTO
Simulation
Results
Conclusion
s
psps
p ps
ps ps
ps hyd ps ps hyd
2
ps ps p
2
ps ps hy
s a p
d
s e ps a ps ps e
2
a e
2
t t
2 2a a a
hyd2 2 2
t t t
12
mechanical power requirement
P V I R I K I R I I K
R K
b b
K K
R R R
b b Q b
K K
Q
K
Q
Q
ps pshydQ x u
57 of 123
58. 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
Optimization
Problem
Notation
Cost
Functional
Terminal
Conditon
Collision
Avoidance
Desired
Trajectories
Complete
Problem
PRONTO
Simulation
Results
Conclusion
T
[ ] [ ]
f
i i
T x x
[ ]
f
i
x
i
58 of 123
59. 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
Optimization
Problem
Notation
Cost
Functional
Terminal
Conditon
Collision
Avoidance
Desired
Trajectories
Complete
Problem
PRONTO
Simulation
Results
Conclusion
59 of 123
60. 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
Optimization
Problem
Notation
Cost
Functional
Terminal
Conditon
Collision
Avoidance
Desired
Trajectories
Complete
Problem
PRONTO
Simulation
Results
Conclusion
2 2[
co
] [ ] [ ] [ ]
1 1 2 2[ ] [ ]
2 2l
c c
, : 1
2 2
0
i j i j
i j
t t t t
c t t
r r
x x x x
x x
2 2 2[ ] [ ] [ ] [ ]
1 1 2 2 c 0,2i j i j
tt t t t Tr x x x x
i j
60 of 123
61. 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
Optimization
Problem
Notation
Cost
Functional
Terminal
Conditon
Collision
Avoidance
Desired
Trajectories
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 t
c t
r r
x o x o
x o
o o
i k
2 2 2[ ] [ ] [ ] [ ] [ ]
1 1 2 2 c 3 0,i k i k k
t t r t T x o x o o
61 of 123
62. Desired Trajectories
• 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
Optimization
Problem
Notation
Cost
Functional
Terminal
Conditon
Collision
Avoidance
Desired
Trajectories
Complete
Problem
PRONTO
Simulation
Results
Conclusion
[ ] [ ]
des des,i i
x u
2L
T T
2 2[ ] [ ] [ ] [ ] [ ] [ ]
des des des
1 1
, ,
2 2
i i i i i i
Q R
l t t t t t t t x u x x u u
TQ TR
62 of 123
63. 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
Optimization
Problem
Notation
Cost
Functional
Terminal
Conditon
Collision
Avoidance
Desired
Trajectories
Complete
Problem
PRONTO
Simulation
Results
Conclusion
[ ] [ ] [ ] [
pow des
]
10
min , , ,v
T N i i i i
i
l l d m T
x u x u x
[ ] [ ] [ ] [ ] [ ] [ ] [ ]
0 f, , 0i i i i i i i
f 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 k
c t k N x o ,
63 of 123
65. 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
m
u 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 ,
n
t x
min h
T
,
0
0
x T ,t f t u t
[by courtesy of J. Hauser] 65 of 123
66. 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
Minimization
of Trajectory
Functionals
Projection
Operator
Approach
Trajectory
Manifold
Equivalence
Newton
Method
Derivatives
Summary
Barrier
Function
Simulation
Results
Conclusion
u t
u x
[by courtesy of J. Hauser] 66 of 123
67. 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
Minimization
of Trajectory
Functionals
Projection
Operator
Approach
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
[by courtesy of J. Hauser] 67 of 123
68. 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
Minimization
of Trajectory
Functionals
Projection
Operator
Approach
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
[by courtesy of J. Hauser] 68 of 123
69. 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
Minimization
of Trajectory
Functionals
Projection
Operator
Approach
Trajectory
Manifold
Equivalence
Newton
Method
Derivatives
Summary
Barrier
Function
Simulation
Results
Conclusion
P P P
0
f r
C K
0 T
P
P r
C
L
L
P T dom P
T P
[Figure by courtesy of A. Saccon]
[by courtesy of J. Hauser] 69 of 123
70. 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
Minimization
of Trajectory
Functionals
Projection
Operator
Approach
Trajectory
Manifold
Equivalence
Newton
Method
Derivatives
Summary
Barrier
Function
Simulation
Results
Conclusion
x x u
0,T K
P
0K
[Figure by courtesy of A. Saccon]
[by courtesy of J. Hauser] 70 of 123
71. 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
Minimization
of Trajectory
Functionals
Projection
Operator
Approach
Trajectory
Manifold
Equivalence
Newton
Method
Derivatives
Summary
Barrier
Function
Simulation
Results
Conclusion
21
,
2
D D PP P P
D P
0: , 0,x f x u x x
u t K t t x
: 0 0i i i i i
i i i i
z A t z B t v z
v v t K t t z
2
1 1 2 2: , 0 0,y A t y B t w D f t t t y
w K t y
P
2
1 2,D P
K
1 2
2
,
,
,
,
,
,
i i i i i i
x u
z D
y D
v D
w
P P
P
P P
[by courtesy of J. Hauser] 71 of 123
72. 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
Minimization
of Trajectory
Functionals
Projection
Operator
Approach
Trajectory
Manifold
Equivalence
Newton
Method
Derivatives
Summary
Barrier
Function
Simulation
Results
Conclusion
D P
T T
[Figure by courtesy of A. Saccon]
T T T
P
D P
T T
[by courtesy of J. Hauser] 72 of 123
73. 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
Minimization
of Trajectory
Functionals
Projection
Operator
Approach
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
andh g
T U
h
*
T U h
g
U Ug
*
P
[by courtesy of J. Hauser] 73 of 123
74. 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
Minimization
of Trajectory
Functionals
Projection
Operator
Approach
Trajectory
Manifold
Equivalence
Newton
Method
Derivatives
Summary
Barrier
Function
Simulation
Results
Conclusion
t
h T T
T P T T
ig
[by courtesy of J. Hauser] 74 of 123
75. PRojection Operator based Newton method for
Trajectory Optimization (PRONTO)
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
[Figures by courtesy of A. Saccon]
Trajectory Manifold Descent Direction
Line Search Update
75 of 123
76. PRojection Operator based Newton method for
Trajectory Optimization (PRONTO)
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
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
[by courtesy of J. Hauser] 76 of 123
77. PRojection Operator based Newton method for
Trajectory Optimization (PRONTO)
• 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
Minimization
of Trajectory
Functionals
Projection
Operator
Approach
Trajectory
Manifold
Equivalence
Newton
Method
Derivatives
Summary
Barrier
Function
Simulation
Results
Conclusion
i
T T
1
arg min ,
2i
i i i
T
Dh q
T
2
iD g
2
iD g
iq i
T T
[by courtesy of J. Hauser] 77 of 123
78. 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
Minimization
of Trajectory
Functionals
Projection
Operator
Approach
Trajectory
Manifold
Equivalence
Newton
Method
Derivatives
Summary
Barrier
Function
Simulation
Results
Conclusion
2 2
1
2
22 21 1, ,,
Dh D
D g D D Dh D
Dg
D h
P P
P P P P P
i T T
2 2
1 21 1 2
2
2 , ,,
generalizes Lagrange multiplier
Dg Dh
D g Dh hD D
P
DP
g h P
T
T T
[by courtesy of J. Hauser] 78 of 123
79. 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
Minimization
of Trajectory
Functionals
Projection
Operator
Approach
Trajectory
Manifold
Equivalence
Newton
Method
Derivatives
Summary
Barrier
Function
Simulation
Results
Conclusion
2 2
1
0
, , ,
T
D l DDh D d P P
2
1 1 2 2: ,y A t y B t w D f t t t
2
1
0 0
2
1
0
2
0
,
,
,
, ,
, ,
T
c
T T
c
s
T
I
D l s D f s s s dsd
K
I
D 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 ,
[by courtesy of J. Hauser] 79 of 123
80. 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
Minimization
of Trajectory
Functionals
Projection
Operator
Approach
Trajectory
Manifold
Equivalence
Newton
Method
Derivatives
Summary
Barrier
Function
Simulation
Results
Conclusion
2
,D g T T T
1
0
T
z Q S z
d z T Pz T
v S R v
T
T
T
Q S
W t
S R
T
22
1
,
i j i j
n
k
ij k
k
fl
w t t q ttt
2
1 2
m
P x T
x
W
, , , , , ,H x u q t l x u t q f x u T
[by courtesy of J. Hauser] 80 of 123
81. 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
Minimization
of Trajectory
Functionals
Projection
Operator
Approach
Trajectory
Manifold
Equivalence
Newton
Method
Derivatives
Summary
Barrier
Function
Simulation
Results
Conclusion
1 1
0
1 1
mi
0 0
n
2 2
s.t. ,z
T
a z z Q S z
d 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
1
10,P PA PBR BA P P T PP Q
T T
1 T
A A BR S
1 T
Q Q SR S
[by courtesy of J. Hauser] 81 of 123
82. In other words, …
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
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
83. 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
Minimization
of Trajectory
Functionals
Projection
Operator
Approach
Trajectory
Manifold
Equivalence
Newton
Method
Derivatives
Summary
Barrier
Function
βδ barrier
Hockey Stick
Extension
Simulation
Results
Conclusion
P
[by courtesy of J. Hauser] 83 of 123
84. The βδ barrier function
• 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
Minimization
of Trajectory
Functionals
Projection
Operator
Approach
Trajectory
Manifold
Equivalence
Newton
Method
Derivatives
Summary
Barrier
Function
βδ barrier
Hockey Stick
Extension
Simulation
Results
Conclusion
2
1
1
2
log
2
log
z z
z z
z
0 1
: , 0,
0
, ,j
T
j
b c d
minh b
T
T h b
[by courtesy of J. Hauser] 84 of 123
85. The βδ barrier function
• 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
Minimization
of Trajectory
Functionals
Projection
Operator
Approach
Trajectory
Manifold
Equivalence
Newton
Method
Derivatives
Summary
Barrier
Function
βδ barrier
Hockey Stick
Extension
Simulation
Results
Conclusion
z
0,
logz z
,
,g h P P
[by courtesy of J. Hauser] 85 of 123
86. 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
Minimization
of Trajectory
Functionals
Projection
Operator
Approach
Trajectory
Manifold
Equivalence
Newton
Method
Derivatives
Summary
Barrier
Function
βδ barrier
Hockey Stick
Extension
Simulation
Results
Conclusion
1z z
n 0ta h
otherwise
z z
z
z
2
C
86 of 123
87. βδ barrier with hockey stick
Introduction
Path Planning
Minimum
Energy
PRONTO
Minimization
of Trajectory
Functionals
Projection
Operator
Approach
Trajectory
Manifold
Equivalence
Newton
Method
Derivatives
Summary
Barrier
Function
βδ barrier
Hockey Stick
Extension
Simulation
Results
Conclusion
βδ for different values of δ βδ ◦ σ for different δ
87 of 123
89. 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
90. 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
Implementing
the
Cooperative
Motion
Planning
Problem
Bathymetry-
based
Trajectory
Planning
Survey
Scenarios
Obstacle
Avoidance
Conclusion
90 of 123
91. 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
Implementing
the
Cooperative
Motion
Planning
Problem
Bathymetry-
based
Trajectory
Planning
Survey
Scenarios
Obstacle
Avoidance
Conclusion
91 of 123
92. Terrain-based trajectory planning
• 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
Implementing
the
Cooperative
Motion
Planning
Problem
Bathymetry-
based
Trajectory
Planning
Survey
Scenarios
Obstacle
Avoidance
Conclusion
92 of 123
93. Terrain-based trajectory planning
• 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
Implementing
the
Cooperative
Motion
Planning
Problem
Bathymetry-
based
Trajectory
Planning
Survey
Scenarios
Obstacle
Avoidance
Conclusion
93 of 123
94. Terrain-based trajectory planning
• 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
Implementing
the
Cooperative
Motion
Planning
Problem
Bathymetry-
based
Trajectory
Planning
Survey
Scenarios
Obstacle
Avoidance
Conclusion
94 of 123
98. 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
Implementing
the
Cooperative
Motion
Planning
Problem
Bathymetry-
based
Trajectory
Planning
Survey
Scenarios
Obstacle
Avoidance
Conclusion
Cost Map MC(x,y)
10,20
C E2cos ,M M x y
0,1
98 of 123
100. Resulting paths and trajectories
Introduction
Path Planning
Minimum
Energy
PRONTO
Simulation
Results
Implementing
the
Cooperative
Motion
Planning
Problem
Bathymetry-
based
Trajectory
Planning
Survey
Scenarios
Obstacle
Avoidance
Conclusion
Result of A*
(The desired trajectories)
Projected paths
(The initial condition) 100 of 123
102. 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
Implementing
the
Cooperative
Motion
Planning
Problem
Bathymetry-
based
Trajectory
Planning
Survey
Scenarios
Obstacle
Avoidance
Conclusion
102 of 123
103. Braid maneuver vs. lawn mower
• 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
Implementing
the
Cooperative
Motion
Planning
Problem
Bathymetry-
based
Trajectory
Planning
Survey
Scenarios
Obstacle
Avoidance
Conclusion
103 of 123
104. Braid maneuver vs. lawn mower
• 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
Implementing
the
Cooperative
Motion
Planning
Problem
Bathymetry-
based
Trajectory
Planning
Survey
Scenarios
Obstacle
Avoidance
Conclusion
104 of 123
105. 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
Implementing
the
Cooperative
Motion
Planning
Problem
Bathymetry-
based
Trajectory
Planning
Survey
Scenarios
Obstacle
Avoidance
Conclusion
VN
105 of 123
106. Generating a “good” initial condition
• 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
Implementing
the
Cooperative
Motion
Planning
Problem
Bathymetry-
based
Trajectory
Planning
Survey
Scenarios
Obstacle
Avoidance
Conclusion
106 of 123
107. Generating a “good” initial condition
• 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
Implementing
the
Cooperative
Motion
Planning
Problem
Bathymetry-
based
Trajectory
Planning
Survey
Scenarios
Obstacle
Avoidance
Conclusion
107 of 123
109. 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
Implementing
the
Cooperative
Motion
Planning
Problem
Bathymetry-
based
Trajectory
Planning
Survey
Scenarios
Obstacle
Avoidance
Conclusion
109 of 123
110. 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
Implementing
the
Cooperative
Motion
Planning
Problem
Bathymetry-
based
Trajectory
Planning
Survey
Scenarios
Obstacle
Avoidance
Conclusion
110 of 123
111. Obstacle field + formation change
Introduction
Path Planning
Minimum
Energy
PRONTO
Simulation
Results
Implementing
the
Cooperative
Motion
Planning
Problem
Bathymetry-
based
Trajectory
Planning
Survey
Scenarios
Obstacle
Avoidance
Conclusion
111 of 123
113. 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
114. 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
115. 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
116. 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
117. 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
118. Thank you!
Research supported in part by project MORPH of the EU FP7 (grant agreement no. 288704, and
by 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 number
SFRH/BD/68941/2010.
Introduction
Path Planning
Minimum
Energy
PRONTO
Simulation
Results
Conclusion
Recap
Outlook
References
118 of 123
119. 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
120. 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
121. 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
122. References
B. Cited in the presentation (cont‘d)
• 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
123. References
B. Cited in the presentation (cont‘d)
• 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