2011 2nd International Conference on Control, Instrumentation and Automation (ICCIA)
Trajectory Optimization for a High Speed Planing Boat Based on
Gauss Pseudospectral Method
Hassan Salarieh, Mohammad T. Ghorbani.
Abstract—In this paper, the problem of Optimal Trajectory
Planning for a high speed planing boat under nonlinear
equality and inequality path constraints, is addressed. First, a
nonlinear mathematical model of the craft’s dynamic is
constructed. To solve a trajectory optimization problem, we
can utilize the indirect or direct methods. In the indirect
methods, the maximum principle of Pontryagin is used to
transform the optimal control problem into Euler-Lagrange
equations, on the other hand, in the direct methods it is
necessary to transcribe the optimal control problem into a
nonlinear programming problem (NLP) by discretization of
states and controls. The resulted NLP can be solved by well-
developed algorithms such as SNOPT. We use direct method to
optimize the trajectories by solving an optimal control problem
using the Gauss pseudospectral method (GPM). An example of
boat berthing in a circular obstacle environment is presented to
demonstrate the effectiveness of the approach for designing
optimal maneuvers.
I. INTRODUCTION
D uring last years, Advanced electronic navigation
systems has become an indispensable aid to navigate
marine vehicles around the world. However, accidents still
frequently occur at sea. This renders the automization of
steering function so appealing. An unmanned high speed
planing boat can operate in dangerous areas without the risk
of losing human life. This may be anything from
reconnaissance in hostile areas to operations in dangerous
weather conditions [1]. The problem of designing an
autopilot for unmanned marine vehicles may be divided into
two sub problem [2],[3]. The first is off line optimal
trajectory planning and the second, on line path control or
path tracking. In this paper the first problem will be
considered. The trajectory optimization allows generating,
for a marine vehicle, a trajectory that can take into account
fuel consumption, specified final time and many others
requirements based on non-linear system dynamics, actuator
saturations and obstacles avoidance possibilities.
In the area of trajectory optimization for marine vehicles,
Djouani et al [4] worked on the 3-Dof model of a marine
vehicle and defined the trajectory in the earth coordinate
system, as a connection of pieces of Bezier curves. They
used the inverse dynamics to compute the controls that make
Manuscript received October 9, 2011.
Hassan Salarieh is with Sharif University of Technology, Department of
Mechanical Engineering, Tehran, Iran (phone: +98 21 6616 5538; fax: +98
21 6600 0021; e-mail: salarieh@sharif.edu).
Mohammad T. Ghorbani is with Sharif University of Technology,
Department of Mechanical Engineering, Tehran, Iran (e-mail:
mt_ghorbani@ mehr.sharif.ir).
978-1-4673-1690-3/12/$31.00©2011 IEEE
the ship match the desired trajectory. Finally, The discrete
augmented Lagrangian approach was used to compute the
optimal constrained Time-Energy trajectory.
A fictitious trans-Atlantic route was established by
Avgouleas [5] so that fuel consumption was minimized
while certain safety constraints were met. The safety
constraints in his work were the probabilities of slamming
and deck wetness, both of which were not allowed to exceed
prescribed limiting values. The problem formulation adopted
in his work lied in the framework of Dynamic Programming.
Escario et al [6] introduced a new possible solution to the
problem of manoeuvre planning for marine vehicles based
on the use of Ant Colony Extended Optimisation. Cellular
Automata was used to define a heuristic function. Their
approach was tested on different manoeuvre scenarios with
good results.
By Reviewing of previous works on the subject of
trajectory optimization of marine vehicles, we realize that
the majority of them are based on 3-Dof model of large low-
speed displacement ships. There is a high degree of coupling
among the various motions of high-speed planing craft.
Thus, the 3-Dof equations cannot be reliably used for
trajectory predictions, as is commonly done for displacement
craft [7]. In addition, The major drawback of these works is
their long computation time and memory usage.
The trajectory optimization can be solved by optimal
control techniques. There are two ways to resolve optimal
control problems, by direct and indirect methods [8]. The
indirect methods are based on Pontryagin maximum priciple
that transforms the optimal control problem into Euler-
Lagrange equations. On the other hand, the direct methods
transform the optimal control problem into nonlinear
programming problems. In this paper, to solve 4-Dof
trajectory optimization problem for a planing boat, we
address a kind of direct method known as Gauss
Pseudospectral Method (GPM) to transform the optimal
control problem into nonlinear programming problem by
parameterization of the states and the controls. These
parameterization technique have an important role in a
convergence and accuracy of the solution and low
computation time [9].
II. PROPOSED DIRECT METHOD
Consider the following general optimal control problem.
Determine the state, x(t), and control, u(t), that minimize the
cost functional
195
 tf collocation points, defined as the roots of the Kth-degree
J = Φ(x(t 0 ), t 0 , x(t f ), t f ) + ³ g (x(t ), u(t ), t )dt . Legendre polynomial, the initial point τ 0 = −1 , and the
t0
(1) final point τ f = 1 . An approximation to the state, X(τ ) , is
subject to the dynamic constraints
formed with a basis of K+1 Lagrange interpolating

x (t ) = f (x(t ), u(t ), t ) ∈ \ . n polynomials Li (τ ), i = 1, 2,..., K , as follows:

(2)
K
the boundary condition x(τ ) ≈ X(τ ) = ¦ Li (τ )x(τ i ). (10)
i =0
φ (x(t 0 ), t 0 , x(t f ), t f ) = 0 ∈ \q . (3)
where
the inequality path constraints
K τ −τ j
C(x(t ), u(t ), t ) ≤ 0 ∈ \ . c
Li (τ ) = ∏
j = 0, j ≠ i τ i − τ j
. (11)
(4)

Where t 0 is the fixed or free initial time, t f is the fixed or The control is approximated using a basis of K Lagrange
†
interpolating polynomials L i , i = 1, 2,..., K ,
free final time, and t ∈ [t 0 , t f ] . Equations (1)-(4) are
referred as the continuous Bolza problem [10]. The GPM K
method requires a fixed time interval, such as [í1, 1]. The u(τ ) ≈ U (τ ) = ¦ L†i (τ )U (τ i ). (12)
independent variable can be mapped to the general interval i =1
τ ∈ [ −1,1] via the affine transformation
Where
2t t + t0
τ= − f . (5) K τ −τ j
tf −t0 tf −t0 L†i (τ ) = ∏
j =1, j ≠ i τ i − τ j
. (13)

Rewrite the optimal control problem as

The continuous dynamics of (7) are then transcribed into the
J = Φ (x(τ 0 ),τ 0 , x(τ f ), t f ) following set of K algebraic constraints via orthogonal
τ (6) collocation:
tf −t0 f
2 τ³0
+ g (x(τ ), u(τ ); t 0 , t f )d τ .
K
t0 −tf
¦D
i =0
ki X(τ i ) −
2
f ( X(τ k ), U(τ k ),τ k ;t 0 , t f ) = 0. (14)

subject to the constraints

where k = 1,..., K and the differentiation matrix,
dx t f − t 0
= f ( x(τ ), u(τ ),τ ; t 0 , t f ) (7) D ∈ \ k ×( k +1) , consisting of the derivative of each
dτ 2 Lagrange polynomial corresponding to the state at each
collocation point. This matrix can be computed offline as
φ (x(τ 0 ), t 0 , x(τ f ), t f ) = 0 (8) follows:
K

K ∏ (τ k − τ j )
C(x(τ ), u(τ ),τ ; t 0 , t f ) ≤ 0. (9) D ki = Li (τ k ) = ¦
j = 0, j ≠ i ,A
K
.
A =0
∏
j = 0, j ≠ i
(τ i − τ j )
Equations (5)-(9) are called the transformed continuous
Bolza problem.
(15)
In the GPM, This optimal control problem is discretized at
some specific discretization points called the Legendre-
where
Gauss (LG) points, and then transcribed into a nonlinear
program (NLP) by approximating the states and controls k = 1,..., K
using Lagrange interpolating polynomials [10]. The set of N and
discretization points includes K = N í 2 interior LG i = 0,..., K .

196
Fig. 1. reference frames used for 4-Dof modeling of high speed craft with
control forces exerted on it.
By Defining X 0 = X (τ 0 ) and X f = X (τ f ) , X f is The other reference frame shown in Fig. 1. is the body-fixed
calculated via a Gauss quadrature frame ( x b , y b , z b ) which is fixed to the hull. The positive
xb-axis points towards the bow, the positive yb-axis points
Xf = X 0 towards starboard and the positive zb-axis points downwards
[12],[13].
tf −t0 K (16)
+
2
¦ ω f ( X(τ
k =1
k k ), U (τ k ),τ k ; t 0 , t f ). As we see in Fig. 1., it is considered that the vessel stern
has a fixed pitch propeller which can also be oriented so that
the propeller exerts the thrust force (T) on planning boat
hull.
where ωk are the Gauss weights. In addition, (6) can be
In this paper we neglect heave and pitch dynamics by
approximated with a Gauss quadrature, resulting in assuming that the mean heave and pitch (trim) are small. The
4-DOF equations of the planning boat's dynamics may be
J = Φ( X0 , t 0 , Xf , t f ) formulated as follows
tf −t0 K (17)
+
2
¦ω k g ( X k , U k ,τ k ; t 0 , t f ). ­ m [u − vr ] ½ ­-R T + (T ) cos(φ p ) ½
k =1
°m [v + ur ] ° ° (T )sin(φ ) °
The boundary constraint is written as ° ° ° p °
® ¾ =® ¾ +τ H .
° ° (T )sin(φ p ) × z p °
b
° I xx p
(21)
φ ( X 0 , t 0 , X f , t f ) = 0. (18)
°¯ I zzb r °¿ °¯ −(T ) sin(φ p ) × x p °¿
and the path constraint is computed at the LG points as

C( X k , U k ,τ k ; t 0 , t f ) ≤ 0. (k = 1,..., K ) (19) b b
where m is the boat’s mass, [ I xx , I zz ] is the vector of

Equaions (14),(16),(17),(18),(19) form the NLP (20):

moment of inertia about x b and z b respectively. [u ,v ] is
the vector of linear surge and sway velocities and [ p , r ] is
tf −t0 K
the vector of angular roll and yaw rate in body-fixed
Minimize J
X (τ k ), U (τ k )
= Φ ( X0 , t 0 , Xf , t f ) +
2
¦ω k g ( X(τ k ), U (τ k ),τ k )
k =1
(20) coordinate. The dots above u, v, r and p represent the time
­ t −t0 K
° X f − X0 − f
2 i =1
¦ ωi f ( X(τ i ), U(τ i ),τ i ;t 0 ,t f ) = 0 derivatives of the corresponding physical variables. In the
°
°K t −t right hand of (21), R T represents the total (speed
S.t. ®¦ D ki X(τ i ) − 0 f f ( X(τ k ), U (τ k ),τ k ;t 0 , t f ) = 0, (k = 1,..., K )
° i =0 2 dependent) resistance of planing hull. To calculate this term,
° φ ( X0 , t 0 , Xf , t f ) = 0 we use Savitsky pre-planing and Savitsky planing algorithm.
°
¯ C ( X k , U k , τ k ; t 0 , t f ) ≤ 0, ( k = 1,..., K ). Savitsky pre-planing algorithm is useful for estimating the
resistance of a planing hull before it gets “onto the plane”.
whose solution is an approximate solution to the continuous On the other hand, Savitsky planing algorithm is used for
Bolza problem. In this paper to solve this NLP, we use estimating the resistance of planing hulls when in the
SNOPT solvers that were developed by the Stanford planing speed regime [14]. In (21), T represents Thrust force
Systems Optimization Laboratory (SOL) [11]. SNOPT is a and φ p represents propeller turn angle relative to x b axis.
software package for solving large-scale optimization
problems. It is especially effective for linear or nonlinear or [ x p , z p ] is the vector of position of propeller relative to
quadratic programming problems whose functions and
boat’s center of gravity. In addition, τ H denotes the vector of
gradients are expensive to evaluate. The functions should
need not be convex. hydrodynamic force and moments as linear functions of the
velocity and acceleration components as follows
III. PLANNING BOAT DYNAMIC MODEL IN FOUR DEGREES
OF FREEDOM ­ 0 ½
° Y v +Y v +Y p +Y p +Y φ +Y r +Y r °
A. Equations of motion ° v v p p φ r r ° (22)
τH = ® ¾.
Fig. 1. shows a schematic view of the coordinate system K
° v v + K v v + K p

p + K p p + K φ φ + K r r + K 
r
r °

used in the description of the marine craft dynamics. There °¯N vv + N v v + N p p + N p p + N φφ + N r r + N r r ¿°
are two reference frame shown in Fig. 1. The first is Global
( x , y , z ) frame that is fixed to the earth. The positive x- where the coefficients are functions of speed (see [7] for
axis points towards the North, the positive y-axis towards the detailed informations). Note that (21) should be formed as
East, and the positive z-axis towards the centre of the Earth. (2) in order to proceed trajectory optimization.

197
 B. Non-holonomic, acturator constraints TABLE I
BOAT PARAMETERS
in addition to (21), from kinematics of the craft, we have
some Non-holonomic constraints as follows Symbol Parameter Value

L length 8m
φ = p B
m
beam
mass
2.5 m
3500 kg
ψ = r cos(φ ) Ixx moment of inertia
about xb
1290 kg.m2
(23)
x = u × cos(ψ ) − v × (sinψ × cosφ ) Izz moment of inertia 10890 kg.m2
about zb
y = u × sin(ψ ) + v × (cosψ × cosφ ). Tmax maximum thrust 7.5 KN
xp position of propeller 0.25 m
relative to boat’s
where x and y represent position of the craft in the global center of gravity in xb
direction
frame and φ and ψ denote roll and yaw angle of craft in the zp position of propeller 0.1 m
global frame respectively. According to (23), the boat can relative to boat’s
center of gravity in zb
not move to another state following an arbitrary line and can direction
not move directly in all directions and can not spin.
Additionally, we must take into account the actuator m
u (t ) ≤ 15 (29)
constraints. The thrust force is limited to Tmax according to s
(24), so there is no reverse action of the propeller, i.e., we
can not stop the boat. The value of propeller turn angle is deg
limited to ‫ט‬10o. p (t ) ≤ 10 (30)
s
0 ≤ T (t ) ≤ T max (24)
deg
r (t ) ≤ 10 . (31)
φp (t ) ≤ 10° (25) s
In the scenario, the cost function to be minimized is related
to minimum thrust problem[16] as (32).
IV. APPLICATION
In this section, we present a scenario for planing boat, to tf

verify the capability of proposed method for path planning.
TABLE I gives the values of the parameters corresponding
to the boat used in this work.
The initial and finial conditions for this maneuver are
[u ,v , p , r , x , y , ϕ ,ψ ]t =0 = [0, 0, 0, 0, 0, 0, 0, 0]
[u , x , y ,ψ ]t = 60 = [0,300,100, 0].
we also define two circular obstacles to be avoided [15]. In
fact, these obstacles are path inequality constraints in the
form of (4).
102 − (x (t ) − 100) 2 − ( y (t ) − 50) 2 ≤ 0
82 − (x (t ) − 50) 2 − ( y (t ) − 0) 2 ≤ 0.
In order to have a smooth and safe path, we define some
limits on the values of boat’s roll angle and linear and
angular velocities as follows
φ (t ) ≤ 20 deg
J = ³ T (t )dt . (32)
0
Finally, the number of collocation points is 80 points.
Fig. 2. depicts the optimal path generated by proposed
method. As we see, the boat started at origin and oriented
(26) toward the final point without any collision to obstacles.
Finally the boat berthed at the end well. Fig. 3. represents
boat earth-fixed position and linear velocities. As we see,
both initial and final conditions related to position and linear
velocities of boat are satisfied. The surge velocity is also
under the admissible value of 15 m/s.
Fig. 4. represents roll and yaw angles and rates. The safety
constraints of (28)-(31) are all satisfied. As we see, the yaw
(27) angle became zero at the end of path, meaning that the boat
berthed well.
Fig. 5. shows thrust force and propeller turn angle. In
order to reach final point, the boat needs maximum thrust
force and after 48th second, the thrust force becomes zero to
have zero final speed due to resistance forces acted on
planing hull.
(28)

198

Fig. 2. boat optimal path in circular obstacle environment.
Fig. 3. linear position and velocities of boat in the scenario.
Fig. 4. angular position and velocities of boat in the scenario.
199
Fig. 5. The value of thrust and propeller turn angle.
This simulation was run on a laptop with a Windows® 7
Operating System (OS), an Intel Core i5 2.27GHz processor,
and 4GB of Random Access Memory (RAM). The mean
computation time for the 80-node solution was
approximately 60 seconds and the value of cost was 3.6e5.
V. CONCLUSION
In this paper the problem of trajectory optimization for 4-
DOF dynamics of a planning boat has been provided.
Because of intricate dynamics of high-speed planning boats,
hard constraints on states and controls and constraints
imposed by obstacles, convergence towards a global
optimum is difficult to achieve with traditional methods. The
use of gauss pseudo spectral method has some benefits such
as relatively low computational time and storage and high
accuracy of resulted optimum solution. SNOPT solver also
helped us to solve the resulted expensive NLP. An example
of boat berthing, moving on a circular obstacle environment,
designed to show the capability of proposed method.
REFERENCES
[1] Ø. Loe, "Collision avoidance for unmanned surface vehicles," M.S.
thesis, Dept. Eng. Cyber., Norwegian Univ., Trondheim, Norway,
2008.
[2] K. Djouani and Y. Hamam, "Optimal constrained multi-criteria
trajectory planning for ship maneuvering and control," in Conf. Rec.
1993 IEEE Conf. Control Applications, pp. 525-530 vol.2.
[3] G. N. Roberts, "Trends in marine control systems," Annual Reviews in
Control, vol. 32, pp. 263-269, 2008.
[4] K. Djouani and Y. Hamam, "Minimum time-energy trajectory
planning for automatic ship berthing," IEEE J. Oceanic Engineering,
vol. 20, pp. 4-12, 1995.
[5] K. Avgouleas, "Optimal ship routing," M.S. thesis, Dept. Mech. Eng.,
Massachusetts Univ., Massachusetts, USA, 2008.
[6] J. B. Escario, et al., "Optimization of autonomous ship maneuvers
applying swarm intelligence," in 2010 IEEE Int. Conf.on Systems Man
and Cybernetics (SMC), pp. 2603-2610.
[7] E. M. Lewandowski, The dynamics of marine craft: maneuvering and
seakeeping. World Scientific, 2004, pp. 361–375.
[8] J. T. Betts, "Survey of numerical methods for trajectory optimization,"
J. Guidance, Control, and Dynamics, vol. 21, pp. 193-207, 1998.
[9] D. Benson, "A Gauss pseudospectral transcription for optimal
control," Ph.D. dissertation, Dept. Aero. and Astro., Massachusetts
Univ, Massachusetts, United States, 2005.
[10] G. Huntington, "Advancement and analysis of a Gauss pseudospectral
transcription for optimal control problems," Ph.D. dissertation, Dept.
 Aero. and Astro., Massachusetts Univ., Massachusetts, United States,
2007.
[11] P. E. Gill, et al., "SNOPT: An SQP algorithm for large-scale
constrained optimization," SIAM Review, vol. 47, pp. 99-131, 2005.
[12] T. Perez, ‘Ship motion control: course keeping and roll stabilisation
using rudder and fins. New York: Springer-Verlag, 2005, pp. 45-58.
[13] T. I. Fossen, Handbook of Marine Craft Hydrodynamics and Motion
Control. John Wiley & Sons, 2011. pp. 25–53.
[14] Hull speed User Manual. Formation Design Systems Pty Ltd, 1984-
2011.
[15] L. Lewis, "Rapid motion planning and autonomous obstacle
avoidance
for unmanned vehicles," M.S. thesis, Dep. Mech. and Astro. Eng.,
Naval Postgraduate School, Monterey CA, USA, 2006.
[16] D. E. Kirk, Optimal control theory: an introduction. Dover
Publications, 2004. pp. 259–273.

200