View
220
Download
1
Tags:
Embed Size (px)
Citation preview
Multiple Marine Vehile Deconflicted Path Planning with Currents and Communication Constraints
A. Häusler1, R. Ghabcheloo2, A. Pascoal1, A. Aguiar1
1Instituto Superior Técnico, Lisbon, Portugal2Tampere University of Technology, Tampere, Finland
Andreas J. Häusler - IAV 2010 2
Introduction
Efficient algorithms for multiple vehicle path planning are crucial for cooperative control systemsNeed to take into account vehicle dynamics, mission parameters and external influencesExample: Go-To-Formation maneouvre
September 8h, 2010
Andreas J. Häusler - IAV 2010 4
The Go-To-Formation Maneouvre
An initial formation pattern must be established before mission startDeploying the vehicles cannot be done in formation (no hovering capabilities)Vehicles can’t be driven to target positions separately (no hovering capabilities)
September 8h, 2010
Mother Ship
Cur
rent
Andreas J. Häusler - IAV 2010 5
The Go-To-Formation Maneouvre
Need to drive the vehicles to the initial formation in a concerted mannerEnsure simultaneous arrival times and equal speedsEstablish collision avoidance through maintaining a spatial clearance
September 8h, 2010
Mother Ship
Andreas J. Häusler - IAV 2010 6
Path Planning Foundations
September 8h, 2010
0 0( ), ( )p t v t
( ), ( )f fp t v t
Initial position
Final position
( ( )), ( ( )), ( ( ))i i ip t v t a t
Andreas J. Häusler - IAV 2010 7
Polynomial Path Planning
Dispense with absolute time in planning a path (Yakimenko)
Establish timing laws describing the evolution of nominal speed with Spatial and temporal constraints are thereby decoupled and captured by &Choose and as polynomialsPath shape changes with only varying
September 8h, 2010
( )t
( ) [ ( ), ( ), ( )]p x y z
( )p ( ) d dt
( ) ( )p
f
[0, ]f
Andreas J. Häusler - IAV 2010 8
Polynomial Path Planning
Polynomial for each coordinate with degree determined by the number of boundary conditionsTemporal speed and acceleration constraints
Choice for timing law with
September 8h, 2010
0( )
N kxkk
x a
0 0ff
τη τ = η + η τ η
τ
2 2 2( ) ( ) ' ( ) ' ( ) ' ( ) ( ) '( )v x y z p 2( ) ''( ) ( ) '( ) '( ) ( )a p p
(0) (0)v ( ) ( )f fv t
Andreas J. Häusler - IAV 2010 9
Cost Function Considerations
September 8h, 2010
cv
totv
wvT
L
D
H C
Andreas J. Häusler - IAV 2010 10
Cost Function Considerations
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
September 8h, 2010
minv maxv maxa
2
0 0
1( ) ( ) ( ) ( ) ( )
2
f f
w w wD wJ D T v t dt C v t A ma t v t dt
max
Andreas J. Häusler - IAV 2010 11
Multiple Vehicle Path Planning
Instead of trying to minimize the arrival time
interval, we can fix arrival times to be exactly
equal and still get different path shapes
Integrate for
September 8h, 2010
( )
(0) ( ) (0)
( ) (0)( ) (0)
ln( ( ) (0))
i
ii
i
i
i f i f i
i f ifi f i
i f i
t
Andreas J. Häusler - IAV 2010 12
Multiple Vehicle Path Planning
Then we obtain ,
from which the spatial paths are computedThe result is in turn used to compute velocity and accelerationOptimization is done using direct search
September 8h, 2010
( ) (0)
( ) ( )(0)1 ( ) (0)
( ) ( ) (0)
i i
f
i i
f f i f i
t
ti i fi
f i f ii f i f i
t t
t
Andreas J. Häusler - IAV 2010 13
Spatial Deconfliction
September 8h, 2010
1 0 1 0( ), ( )p t v t
2 0 2 0( ), ( )p t v t
1 1( ), ( )f fp t v t
2 2( ), ( )f fp t v t
d E
Initial positions
Final positions(target formation)
Andreas J. Häusler - IAV 2010 14
Temporal Deconfliction
September 8h, 2010
1 0 1 0( ), ( )p t v t
2 0 2 0( ), ( )p t v t
1 1( ), ( )f fp t v t
2 2( ), ( )f fp t v t
Initial positions
Final positions(target formation)
Andreas J. Häusler - IAV 2010 15
Deconfliction
Spatial deconfliction: subject to
For temporal deconfliction, the constraint changes to
Simultaneous arrival at time
September 8h, 2010
; 1, ,1
mini
n
i ii n
i
w J
2 0i k j lp τ p τ E ;E >
2 0i jp t p t E ;E > 1, 0, fi, j = ,n,i j,t t
1, 0, 0,k l f fi ji, j = ,n,i j, τ ,τ τ τ
1 2
opt
, 1
arg minf
n
f i it t t i
t w J
Andreas J. Häusler - IAV 2010 16
Environmental Constraints
Incorporated through barrier function
Path planning with currents for more energy-efficient paths (instead of solely relying on path following controller)Path planning with communication constraints for range keeping within the group
September 8h, 2010
log( )
( ) 11 log( )
1
k
z z
z k z kz
k k
(Hauser, CDC 2006)
Andreas J. Häusler - IAV 2010 17
Path Planning System
September 8h, 2010
AMVData
Path Generation Optimization
Algorithm
Init. Position & Heading
Initial Velocity
Final Position & Heading
Final Velocity
Initial Guess
Dynamical Constraints
Mission Specifications
Environmental Constraints
Path
Revised Guess
Nominal Paths
Speed Profiles
Tim
e-Coordinated P
ath Follow
ing
Spatial Clearance
Comm. Constraint
Cost Criterion
Current
Obstacles
Andreas J. Häusler - IAV 2010 18
Time-Coordinated Path Following
Closed-form solution to problems of open-loop
path planning and trajectory tracking
Using virtual time with
September 8h, 2010
0,1ft t
( ) (0)
ln 1 ( ) (0) 1( ) (0)
ln ( ) (0)
i i
i i
i
i
i f i f i
i f i i ff
i f i
i f i
t t
(Ghabcheloo, ACC 2009)
Andreas J. Häusler - IAV 2010 19
Time-Coordinated Path Following
Path following control strategy
Guarantees that vehicles and are deconflicted and that they follow their trajectory if
A time coordination control law can be found so that the mission remains “near optimal“ in the presence of disturbances
September 8h, 2010
( ) ( ) ( ( ))i ii ie t t p t p p
0, ( ) ( )i jt t t i j
( )i t t
Andreas J. Häusler - IAV 2010 20
Simulation Results
Algorithm makes use of currents to minimize expected energy usageSolid lines = vw, dashed lines = vdes and dotted lines = vmin and vmax
September 8h, 2010
Andreas J. Häusler - IAV 2010 21
Simulation Results
Algorithm makes use of currents to minimize expected energy usageSolid lines = vw, dashed lines = vdes and dotted lines = vmin and vmax
September 8h, 2010
Andreas J. Häusler - IAV 2010 22
Simulation Results
Communication constraints with and without spatial deconflictionCommunication break distance 50m and 80m
September 8h, 2010
Andreas J. Häusler - IAV 2010 23
Conclusions
Recent improvements of the planner include currents and communication constraintsThereby enhances usability for mission planningBarrier function approach allows for “probing” the path limits (useful for obstacle avoidance)Algorithm easily adaptable for arbitrary number of vehicles
September 8h, 2010
Andreas J. Häusler - IAV 2010 24
Future Work
Running time increases exponentially with number of discrete points use different optimization approach and/or different means of path descriptionIncorporate real vehicle dynamics instead of approximation through constraintsImprove communication constraintUse more sophisticated energy usage equationAllow for specification of vehicle sub-groups
September 8h, 2010
Andreas J. Häusler - IAV 2010 25
Thank you for your attention!
September 8h, 2010
Outlook: trajectory optimization using a projection operator approach
(blue: desired path, green: time and energy optimal path according to vehicle dynamics
Outlook: obstacle avoidance using the projection operator
Outlook: collision avoidance using the projection operatorOutlook: obstacle and collision avoidanceusing a projection operator optimization
approach