An introduction on applying the PRONTO toolkit to trajectory planning problems, given in the course of Prof. John Hauser's class on Optimal Control at CU Boulder.

1. Multiple AMV Trajectory
Planning Using the PRONTO TK
Andreas J. Häusler
Instituto Superior Técnico
Institute for Systems and Robotics
Dyanamical Systems and Ocean Robotics Laboratory
Lisbon, Portugal

2. About me
From Bavaria, but living in Portugal for
almost 5 years
PhD Student in Marine Robotics at
Instituto Superior Técnico
(Lisbon, Portugal)
Currently working on Trajectory
Planning and Generation for Multiple
Marine Vehicles
CU Boulder, May 4, 2012 2A. Häusler

3. Some Not-So-Important Trivia
Arrived at Boulder in
January
with wife and daughter
will return to Lisbon in
roughly a week
had a great time at CU and
in Colorado
CU Boulder, May 4, 2012 A. Häusler 3
The Häusler’s coat of arms (1632)

4. Introduction
Efficient algorithms for multiple vehicle
trajectory planning are crucial for cooperative
control systems
Need to take into account vehicle dynamics,
mission parameters and external influences
Aim for minimum energy usage
CU Boulder, May 4, 2012 4A. Häusler

5. Path Planning System
MULTIPLE VEHICLE
PATH PLANNING
SYSTEM
Initial Positions
Initial Velocities
Final Positions
Final Velocities
Cost Criterion (e.g.
weighted sum of
energies,
maneuvering time)
Vehicle Model
&
Hydrodynamics
Collision
Avoidance
Constraints
Mission Parameters
(e.g. obstacles,
bathymetric map)
Feasible
Trajectories
CU Boulder, May 4, 2012 5A. Häusler

6. What it Really Looks Like
CU Boulder, May 4, 2012 A. Häusler 6
Initial State & Input
Final State & Input
Initial Guess
Dynamics
Trajectory
Revised Guess
Expected Energy
Trajectories
Safety Distance
Comm. Constraint
Cost Criterion
Current
Obstacles
Desired Trajectory
BatymetricData
AMV
Data
Projection
Operator
“Magic”
Newton
Method
Mission
Specifications
CoordinatedTrajectory
TrackingController
Pre-
Planner
Environmental
Constraints
SupervisingMission
Operator

7. The MEDUSA ASV
Semi-Submersible
Two Thrusters (Seabotix
HPDC 1507)
1035mm × 300mm ×
840mm total
Max. Speed 1.5 m/s
Originally designed for
diver assistance/harbor
patrolling scenarios
CU Boulder, May 4, 2012 A. Häusler 7

8. Dynamic Model
Assumption: since the MEDUSA is a surface craft,
we can safely consider only 2D dynamics (i.e.
ignore roll and pitch dynamics)
The vessel is controlled with common and
differential thrust (two thrusters, no rudders)
This gives us 3 kinematic states + 3 dynamic
states + 2 inputs
CU Boulder, May 4, 2012 A. Häusler 8

9. Dynamic Model
Kinematics
𝑥
𝑦
𝜓
=
cos 𝜓 − sin 𝜓 0
sin 𝜓 cos 𝜓 0
0 0 1
𝑥
𝑦
𝜓
Dynamics
𝑀𝑟𝑏 + 𝑀 𝑎 𝜈 + 𝐶𝑟𝑏 𝜈 − 𝐶 𝑎 𝜈 𝜈 + 𝐷 + 𝐷 𝑛 𝜈 𝜈 = 𝜏
where 𝜈 =
𝑢
𝑣
𝑟
and 𝜏 =
𝑇𝑝𝑠 + 𝑇𝑠𝑏
0
𝑙 𝑇𝑝𝑠 − 𝑇𝑠𝑏
Inputs 𝑛 𝑝𝑠 and 𝑛 𝑠𝑏
CU Boulder, May 4, 2012 A. Häusler 9

10. Thruster Model
Due to design: negligible propeller-hull
interaction
Four-quadrant propeller model
𝑇 =
1
2
𝜌𝑐 𝑇 𝛽 𝑉𝑎
2
+ 𝑉𝑝
2
𝜋𝑅2
𝑄 =
1
2
𝜌𝑐 𝑄 𝛽 𝑉𝑎
2
+ 𝑉𝑝
2
𝜋𝑅2
𝑑
where
𝑉𝑎 𝑝𝑠
= sin atan2 −𝑝 𝑦, 𝑝 𝑥 𝑙𝑟 + 𝑢 = −𝑝 𝑦 𝑟 + 𝑢
𝑉𝑝 𝑝𝑠
= 0.7𝑅𝜔 𝑝𝑠 = 0.7𝑅2𝜋𝑛 𝑝𝑠
CU Boulder, May 4, 2012 A. Häusler 10

11. DC Motor Model
Standard DC motor equations
𝐿 𝑎
𝑑𝐼
𝑑𝑡
+ 𝑅 𝑎 𝐼 = 𝑉 − 𝐾𝑒 𝜔
𝐽 𝑚 𝜔 + 𝑏𝜔 = 𝐾𝑡 𝐼 𝑎 − 𝑄
For now, assume sufficiently fast dynamics →
steady state equation for voltage
𝑉 = 𝑅 𝑎 𝐼 + 𝐾𝑒 𝜔 = 𝑅 𝑎 𝐼 + 𝐾𝑒2𝜋𝑛
Current map obtained from measurements
𝐼 𝑝𝑠 𝑛 𝑝𝑠 = 𝑎𝑛 𝑝𝑠
3
+ 𝑏𝑛 𝑝𝑠
2
+ 𝑐𝑛 𝑝𝑠 + 𝑑
CU Boulder, May 4, 2012 A. Häusler 11

12. Cost and Constraints
Using the motor model, the instantaneous power
requirement is
𝑃𝑡𝑜𝑡 = 𝑉𝑝 𝑠 𝐼 𝑝𝑠 + 𝑉𝑠𝑏 𝐼𝑠𝑏 + 𝑃𝑝
Additionally: desired traj. (using 𝐿2 minimization)
Additionally: inter-vehicle collision and obstacle
avoidance constraints (using log barrier function)
𝑥 𝑖
𝑡 − 𝑥 𝑗
𝑡
2
2𝑟𝑐
2
+
𝑦 𝑖
𝑡 − 𝑦 𝑗
𝑡
2
2𝑟𝑐
2
− 1 ≥ 0
CU Boulder, May 4, 2012 A. Häusler 12

13. Results
Scenario: 3 Medusas
and 2 obstacles
Desired trajectories
passing through
obstacles & causing
collisions
Result: collision-free
trajectories through
narrow passage
CU Boulder, May 4, 2012 A. Häusler 13

14. Illustration: Armijo “Exploration”
CU Boulder, May 4, 2012 A. Häusler 14

15. Illustration: “Morphing” Trajectories
CU Boulder, May 4, 2012 A. Häusler 15

16. Illustration: Trajectory “Tracking”
CU Boulder, May 4, 2012 A. Häusler 16

17. Open Issues & Outlook
Issues with the four-quadrant model still prevent
2nd order descent
Gains need to be tweaked
Incorporate mission performance metrics à la
Naomi Leonard
Use a (propably topology-based) pre-planning
method for global optimal (in some discrete
sense) initial guess
CU Boulder, May 4, 2012 A. Häusler 17

18. Questions? Comments? Suggestions?
CU Boulder, May 4, 2012 A. Häusler 18