Trajectory Tracking Nonlinear Model Predictive Control forAutonomous Surface Craft

Bruno J. Guerreiro, Carlos Silvestre, Rita Cunha, and António Pascoal

Abstract— This paper presents a solution to the problem oftrajectory-tracking control for autonomous surface craft (ASC)in the presence of ocean currents. The proposed solution isrooted in nonlinear model predictive control (NMPC) tech-niques and addresses explicitly state and input constraints.Whereas state saturation constraints are added to the underly-ing optimization cost functional as penalties, input saturationconstraints are made intrinsic to the nonlinear model used in theoptimization problem, thus reducing the computational burdenof the resulting NMPC algorithm. Simulation and experimentalresults show that the NMPC strategy adopted yields goodperformance in the presence of constant currents and validatethe real-time implementation of the proposed techniques.

I. INTRODUCTION

This paper addresses the problem of trajectory-trackingcontrol of an autonomous surface craft (ASC) under theeffect of constant ocean currents, taking explicitly into ac-count the physical limitations of the vehicle. The increasingdemand by marine scientists for adequate technological toolsto sample the ocean at appropriate temporal and spatial scalesmotivates the use of ASCs capable of automatically acquiringand transmitting large data sets to one or more support unitsinstalled on shore. In the future, this practical setup will en-able scientists to control the execution of sea missions fromthe security and comfort of their laboratories. trajectory-tracking controllers are traditionally based on a two-stepdesign methodology: a fast inner loop that stabilizes thevehicle’s attitude and, using a time-scale separation criterion,a slower outer loop that relies on the kinematic equationsof the vehicle and converts the tracking errors into innerloop commands. An integrated approach to the design ofinner-outer loop control structures for autonomous vehiclesmoving in 3D space was proposed in [1] and [2]. Themethodology adopted relied on linearization techniques forlinear controller design about trimming trajectories, togetherwith gain scheduling techniques to switch among the linearcontrollers. The interested reader is referred to [3] and [4]for a discussion of topics related to this circle of ideas, and[5] for the application of similar techniques to the control ofthe DELFIMx ASC. It is also worth pointing out that some

This work was partially supported by project FCT [PEst-OE/EEI/LA0009/2011] and by the EU Project TRIDENT (ContractNo. 248497). The work of Bruno Guerreiro was supported by the PhDStudent Grant SFRH/BD/21781/2005 from the Portuguese FCT POCTIprogramme.

The authors are with the Institute for Systems and Robotics, InstitutoSuperior Técnico, at the Technical University of Lisbon, Av. Rovisco Pais1, 1049-001 Lisboa, Portugal. C. Silvestre is also with the Faculty of Scienceand Technology, University of Macau, Taipa, Macau.{bguerreiro,cjs,rita,antonio}@isr.ist.utl.pt

Fig. 1. Delfim ASC system sea trials.

authors use nonlinear or adaptive control techniques to tacklethe ASC control problem [6], [7].

The design methodology proposed is formulated in thescope of nonlinear model-based predictive control (NMPC)(see [8] and [9]), in line with the methodology developed in[10] for an autonomous rotorcraft. The control law adoptedhere is obtained by solving on-line, at each sampling instant,a finite horizon open-loop optimal control problem and usingthe actual state of the vehicle as the initial state. The resultingoptimization problem is solved numerically using the quasi-Newton method to compute search directions and resortingto the Wolfe conditions in a line search algorithm to solvea step size optimization subproblem [11]. Throughout thiswork a nonlinear dynamic model of an ASC derived fromfirst physics principles is used. Based on the nonlinear modelderived, a trajectory-tracking error-space model is proposedthat, when linearized about trimming trajectories, yields atime-invariant system. Furthermore, the intrinsic physicallimitations of the actuators are incorporated in the designmodel using smooth saturation functions. This improvedmodel allows for the optimization algorithm to generate validcontrol actions, even without using constraints in the costfunctional. The key contributions of this paper include: i) theuse of a conveniently defined trajectory-tracking error-spacethat is rooted in a physically sound nonlinear vehicle model,ii) the incorporation of the intrinsic physical limitations ofthe vehicle into the design model; iii) the use of simpleand well established optimization techniques to solve thetrajectory-tracking NMPC problem for a full nonlinear modelof an ASC under constant disturbances, yielding a controllerstructure that lends itself to real-time implementation; and iv)the simulation and experimental evaluation of the methodol-ogy, providing insightful information about the performanceof this strategy and its real-time implementation using theDELFIM catamaran ASC, shown in Fig. 1.

The paper is organized as follows. Section II presents a

summary of the ASC dynamic model, Section III formulatesthe NMPC problem by describing the control problem, theconstraints, and the optimization algorithms. Simulation andexperimental results from sea trials are presented in SectionIV, whereas Section V contains the main conclusions anddiscusses issues that warrant further research work.

II. CATAMARAN MODEL

This section describes the dynamic model of an ASC,which has two hulls, two propellers driven by electricalmotors, and a submerged torpedo-shaped sensor container,attached to the vehicle by a central wing-shaped structure(see [12] and [13] for an in-depth presentation of this modeland a description of the catamaran surface craft). Adoptingstandard notation in the field, let {I} denote an inertialcoordinate frame and {B} a body fixed coordinate frameattached to the vehicle’s center of mass. Further consider theposition IpB = [x y]T of the origin of {B} with respect to{I}, the linear velocity v = [u v]T of frame {B} relative to{I} and expressed in {B}, the heading angle ψ that describesthe orientation of frame {B} with respect to {I}, and theangular velocity r of frame {B} relative to {I}, expressedin {B}. In what follows, let the generalized variables for thehorizontal plane motion be given by ν =

�u v r

�T, η =�

x y ψ�T

, and τ =�X Y N

�T, which denote the

generalized velocity, position and force vectors, respectively.The actuation vector is given by n = [nc nd]

T , wherenc and nd denote the common and differential modes ofthe propellers’ speed of rotation, respectively. With thisnotation, the generalized equations of motion for the vehiclekinematics and dynamics are defined by

η = J(η)ν ,

M ν +C(ν)ν = τ (ν,ν,n) ,

where J(η) is the rotation matrix from {B} to {I}, M isthe rigid body inertia matrix, and C is the matrix of Coriolisand centripetal terms.

A model that captures the effect of constant currents onthe ASC dynamics can be obtained by rewriting the aboveequations in terms of the vehicle’s velocity relative to thefluid. It is assumed that the generalized velocity results fromthe sum of two components ν = νr + J(η)−1 νf , whereνr is the vehicle’s generalized velocity with respect to thefluid expressed in the body frame {B} and νf is the fluidgeneralized velocity described in the inertial frame {I}. Toobtain the new dynamic equations depending on νr insteadof ν, note that the generalized force τ can be decomposedas τ (νr,νr,n) = M2 νr + τ 1(νr,n), where M2 is aconstant parameter matrix. Considering that τ 2(νr,n) :=−C(νr)νr + τ 1(νr,n) and M3 := M−M2 is a full rankmatrix, it is a matter of algebraic manipulation to show thatthe kinematic and dynamic equations of motion are given by

η = J(η)νr + νf , (1)

νr = M−13 τ 2(νr,n) . (2)

A. Generalized Error Dynamics

This section presents a generalized error-space to describethe vehicle’s motion about trimming trajectories in the ab-sence of currents. Consider the equations of motion presentedin (1) and (2) without the effect of constant currents (thatis νf = 0), yielding νr = ν, and let νc, ηc, and nc

denote the trimming values of the state and input vectors.At trimming, the generalized velocity satisfies νc = 0,implying that nc = 0. It can be shown that the trimmingtrajectories are straight lines and circles described by thevehicle at constant speed. These trajectories can be fullydescribed by the parameter vector ξ = [Vc rc]

T whereVc = �vc�2 =

√u2 + v2. Therefore, ξ fully parameterizes

the set of achievable trimming trajectories.The generalized error vector between the vehicle state and

the desired trajectory is defined as

xe =

νe

ηe

xi

=

ν − νc

J−1(η) (η − ηc)� t

0Πηedt

,

where Π denotes the projection matrix Π =�I2×2 02×1

�.

Note that designing the controller to drive the error vectorcomponent xi to zero will provide integral action. In the newcoordinates, the error dynamics take the form

νe = νηe = ν − J−1(ηe)νc −Q(ν)ηe

xi = Πηe

, (3)

where Q(νr) = S([0 0 r]T ) and S(a) stands for the skew-symmetric matrix that verifies S(a)b = a×b. Using (3), itis straightforward to show that the linearization of the errordynamics about νe = 0 and ne := n − nc = 0 is timeinvariant. The ASC error model described in (3) can also berewritten as

xe = fe (xe,ne) .

In most practical mission scenarios involving ASCs, the onlyavailable velocity measurements are those of the velocityof the vehicle relative to the fluid, provided by a Doppler.Therefore, the implementation of this error-space uses thevelocity relative to the fluid νr, that is, νe � νr − νC .

B. Intrinsic Input Saturation

As only the error states and inputs are used within theoptimization problem, the definition of input constraints onthe actual vehicle inputs could be achieved either by includ-ing additional constraints or by using variable constraints forthe error inputs at each sampling instant along the predictionhorizon. Alternatively, even complex physical constraints canbe easily incorporated in the nonlinear design model of thevehicle, as decribed below.

Let the new inputs n = [nc nd]T be defined as smoothly

saturated functions of the regular inputs n = [nc nd]T ,

so that the dynamic equation is now given by νr =M−1

3 τ 2(νr, n(n)). The saturation functions are derivedfrom the basic function a(a) = a

1+|a| , applying translationsand scaling both to the function and its derivative, such that

inside the bounds a = a and outside the bounds a tendssmoothly to the maximum value amax or the minimum valueamin. For the type of vehicle considered in this work, it isnecessary to impose a minimum value for the common modeinput nc, and also bounds for the differential input, whichdepend on the current value of the common mode input. Thesaturation function of the common mode input is defined as

nc(nc) =

nc , �c ≤ nc ≤ �c

�c +nc−�c

1+nc−�c�

, nc > �c

�c +nc−�c

1−nc−�c�

, nc < �c

,

with �c = ncmax− � and �c = ncmin

+ �, where 0 < � < 1is a constant (typically 0.01), that defines the length of thesmooth transition. The saturation of the differential inputis given by the function nd(nc, nd), obtained using thesame approach presented above. In brief, considering thatn ∈ N ⊂ Rnn , the procedure described above defines thenew saturated input vector as n ∈ Rnn , simplifying theoptimization problem formulation.

C. Discretization and Delay Modeling

In what follows, the control problem is formulated as adiscrete-time open-loop optimal control problem. For thisreason, the equations of motion of the vehicle are describedas difference equations. Considering the notation ak :=a(k Ts), for some time-dependent vector a(t) and sampletime Ts, the difference system equations are obtained usingthe forward Euler discretization, yielding

xek+1≈ xek + Ts fe(xek ,nek)) = fd(xek ,nek) .

To model the delay between the instant the state variablesxek are measured and the instant a new control actionnek+1

is made available, the model is augmented with anextra delay state. Considering the new state vector xk :=[xT

ekxTnk]T , the input vector uk := nek , and f(xk,uk) :=

[ fd(xek ,xnk)T nT

ek ]T , the model takes the form

xk+1 = f (xk,uk) . (4)

III. MODEL PREDICTIVE CONTROL PROBLEM

In this section the NMPC problem is formulated as adiscrete-time open-loop optimal control problem with finitehorizon, subject to the discrete nonlinear model equations aswell as state and input saturation constraints. From (4), thevehicle dynamics can be modeled as a discrete-time state-space equation with state xk ∈ X and input uk ∈ U , whereX ⊂ Rnx and U ⊂ Rnu denote the sets of admissible stateand control vectors, respectively. At each instant of time, theNMPC algorithm uses the nonlinear model of the vehicle andthe current state to predict the evolution of the system withina predefined time horizon. For simplicity, each instant k isconsidered to be the initial instant of the horizon prediction,so that in the rest of this section xk+i and uk+i are denotedas xi and ui, respectively. Let N be the prediction horizon ofthe control problem, U = {u0, . . . ,uN−1} the sequence ofcontrol inputs, and X = {x0, . . . ,xN} the sequence of statevectors generated by that control sequence. The saturation

constraints for the state and input sequences are defined bythe conditions X ∈ XN and U ∈ UN , where XN = {X :xi ∈ X , ∀i=0,...,N} and UN = {U : ui ∈ U , ∀i=0,...,N−1}.Using (4) and denoting the model function as fi := f(xi,ui),the model constraint can be written as

FM (X,U) =�(f0 − x1)

T · · · (fN−1 − xN )T�T

= 0 .

Given these constraints, the NMPC problem can be definedas the nonlinear optimization problem

U∗ = argminU

J (5)

s.t. X ∈ XN , U ∈ UN (6)FM (X,U) = 0 (7)

where J = FN +�N−1

i=0 Li, Fi = 12 x

Ti Pxi, Li =

12

�xTi Qxi + uT

i Rui

�, whereas P, Q, and R are symmet-

ric positive definite matrices. In brief, the NMPC objectiveis to find, at each instant k, the optimal control sequence U∗

with horizon N , such that the resulting state sequence X∗

together with U∗ minimize the cost functional J withoutviolating the state and input constraints imposed by (6).Following a by now standard approach, the constrainedoptimization problem presented above can be solved byreformulating it as an unconstrained optimization problemand using gradient methods to estimate the optimal solution.

A. State and Input Saturation Constraint

The saturation constraints defined in (6) are included inthe optimization problem to complement the intrinsic inputconstraints described in Section II-B and enable the definitionof mission specific bounds for both state and input vectors.These constraints can be incorporated in the cost functionalas a penalty function FR (x,u) , which is zero-valued for x ∈X and u ∈ U and behaves as a quadratic function outsidethese sets. Defining the feasibility sets for state and inputvectors as X = {x ∈ Rnx : x(j) ≤ x(j) ≤ x(j) ∀j=1,...,nx

}and U = {u ∈ Rnu : u(l) ≤ u(l) ≤ u(l) ∀l=1,...,nu

},respectively, the penalty function is defined as

FS(x,u) =

nx�

j=1

fS(x(j)) +

nu�

l=1

fS(u(l)) ,

where fS(a) :=12 h

2(|a−acenter|−arange)wa, acenter := (a+a)/2, arange := a − acenter, wa is a positive scalar weight,whereas h(a) = a if a > 0, and h(a) = 0 otherwise.

B. Unconstrained Optimization Problem

Adding the saturation constraints to the optimization costfunctional, the new problem can be written as

U∗ = argminU

J (8)

s.t. FM (X,U) = 0 (9)

where J = FN+�N−1

i=0 Li ,, Fi = Fi+FR(xi,0) and Li =Li + FR(xi,ui). The elimination method using Lagrangemultipliers is used to solve the model constraint (9). Intro-ducing the Lagrange multiplier sequence Λ = {λ1, . . . ,λN}and the Hamiltonian Hi = H (xi,ui) = Li + λT

i+1 fi, after

some algebraic manipulations, the cost functional J can berewritten as

J = FN − λTN xN +

N−1�

i=1

�Hi − λT

i xi

�+H0.

For a fixed initial state x0, the first order conditions ofoptimality yield

∂ J

∂xi=

∂Hi

∂xi− λi = 0 , ∀i=1,...,N−1 , (10)

∂ J

∂xN=

∂ FN

∂xN− λN = 0 , (11)

∂ J

∂ui=

∂Hi

∂ui= 0 , ∀i=0,...,N−1 , (12)

where ∂ Hi

∂ui= ∂ Li

∂ui+ ∂ fi

∂uiλi+1 and ∂ Hi

∂xi= ∂ Li

∂xi+ ∂ fi

∂xiλi+1.

Because in the cost functional the Lagrange multiplierssequence is multiplied by zero value terms, λi+1(fi−xi+1),they can be arbitrarily chosen. In particular, by choosingλN = ∂ FN

∂xNand λi = ∂ Hi

∂xi, for all i = N − 1, . . . , 1, the

first order conditions of optimality reduce to (12). Thus, aniterative algorithm based on the first order gradient methodcan be readily applied to estimate U∗, whereby at eachoptimization iteration j, the control sequence is updatedaccording to

U(j+1) = U(j) + sΔ(j) , (13)

where s denotes the step size and Δ(j) the search direction.The optimization algorithm can be summarized as follows.

Algorithm 1: Minimization algorithm for the NMPC un-constrained problem.

1) Initialize X(0), U(0) and j = 0;2) Compute {λi} , i = N, . . . , 1;3) Compute

�∂ Hi

∂ui

�, i = 0, . . . , N − 1;

4) Compute the search direction Δ(j) ;5) Compute the step size s using Wolfe conditions;6) Compute U(j+1) using (13) and X(j+1) = {xi} using

xi+1 = f (xi,ui), for i = 0, . . . , N − 1;7) If �∇J (j)|(j)� ≥ ε: repeat from (2); else, apply u0 to

system and set U(0) = {u1, . . . , uN−1}.The search direction is obtained using the quasi-Newtonmethod, Δ(j) = −D(j) ∇H(j)|U(j) , where ∇H(j)|U(j)

is the sequence of vectorized Hamiltonian derivatives

vec�

∂ H(j)i

∂ui

�, for all i = 0, . . . , N − 1 , and D(j) is an

estimate of the inverse matrix of the second-order derivativeof the Hamiltonian sequence, as detailed in [11].

The line search optimization subproblem is numericallysolved using the Wolfe rule. This approach guarantees adecrease of the cost functional, as the well known Armijorule does, and ensures reasonable progress by ruling outunacceptably short steps [11]. Consider the step size opti-mization subproblem defined by

s∗ = argmins≥0

φ(s) ,

where φ(s) = J (j+1) = J�X(j+1),U(j+1)

�, with U(j+1)

as in (13) and its derivative is given by φ�(s) = d J(j+1)

ds .

The Wolfe algorithm finds an acceptable step size, which isan estimate of the optimal step size.

A formal stability analysis of the proposed NMPC method-ology is beyond the scope of this paper, noting that signifi-cant work on related approaches is available in the literature(see [8] and references therein). From the literature, it can beconcluded that the stability of the proposed algorithm relieson the choice of the horizon N , the parameter matrices P,Q, and R, as well as the terminal cost function, F (xN ). Insummary, considering a locally stabilizing terminal controllaw, κf (x), and respective positively invariant terminal setXf , it must be guaranteed that the system converges to theterminal set, which, by definition, ensures convergence to theequilibrium point if the terminal control law is used, throughan appropriate definition of F (xN ). Although necessary forthe formal convergence analysis, most NMPC techniques donot require the explicit use of the terminal set or the terminalcontrol law for the computation of the NMPC control law.

IV. SIMULATION AND EXPERIMENTAL RESULTS

In this section, the performance of the nonlinear NMPCcontroller introduced above is first evaluated in simulationby drawing a comparison with the results obtained witha basic LQR gain switching methodology (see [14] for asimilar approach using H2 synthesis). In the results presentedhereafter, the ASC nonlinear model described in Section II isparameterized for the DELFIMx Catamaran and used bothin the NMPC control algorithm and plant simulation. Thesimulations where carried out in an Intel Pentium Centrinoprocessor at 1.7 GHz, using Matlab/Simulink with C mex-functions. The reference trajectory, defined in the inertialframe, was selected to illustrate the behavior of the controlalgorithms in extreme conditions, which include discontinu-ities in the reference velocities and non zero initial errors,and is composed of three different sections: i) a straight line(�vc� = 1 m/s and rc = 0 rad/s), to be tracked betweenηc = 03×1 and ηc = [72 0 0]�, with initial conditionsν0 = [0.6 0 0]� and η0 = [0 − 1 − π/4]�; ii) onefourth of a circle turning to port side (�vc� = 1.5 m/sand rc = −3 deg/sec); and iii) a complete circle turningto starboard (�vc� = 1 m/s and rc = 1.6 deg/sec). Thesample time is Ts = 0.2 s and the horizon is N = 30sample times, or equivalently, 6 seconds. The precision ofthe solution is determined by the algorithm stop conditions,e. g., |J (j)

k − J(j−1)k | < 10−2.

Two different scenarios were simulated in order tohighlight the major differences between the LQR andNMPC controllers: 1) trajectory-tracking without current;and 2) trajectory-tracking with a constant current (νf =[−0.1 0.2 0]� m/s). The simulation results of these twoscenarios are presented in Fig. 2, showing the trajectoriesdescribed by the ASC using the NMPC and LQR controllers,as well as the time evolution of the position error andthe actuation. It can be seen that both the LQR and theNMPC control methodologies achieve the tracking objective,with or without constant current. However, the LQR methodpresents larger excursions in actuation both at the initial stage

0 20 40 60 80 100 120 140 160

−60

−50

−40

−30

−20

−10

0

x [m]

y [

m]

Ref

LQR

MPC

LQR curr

MPC currv

f [dm/s]

(a) Trajectory

0 50 100 150 200 250 300

−1

0

1

xe(t

) [m

]

0 50 100 150 200 250 300

−4

−2

0

2

4

ye(t

) [m

]

0 50 100 150 200 250 300

−0.5

0

0.5

ψe(t

) [r

ad]

Time[s]

LQR

MPC

LQR curr

MPC curr

(b) Generalized position error

0 50 100 150 200 250 3000

5

10

15

20

25

nc(t

) [r

ad/s

]

0 50 100 150 200 250 300

−80

−60

−40

−20

0

20

40

nd(t

) [r

ad/s

]

Time[s]

LQR

MPC

LQR curr

MPC curr

(c) Actuation

Fig. 2. Simulation results

and during the transitions between sections, which generallytranslate into larger position errors. Moreover, the controleffort demanded by the LQR method is far greater than thatof the NMPC, and it even violates the conditions for validoperation (by having |nd| > nc). The major limitation of theNMPC method is the computation time needed to determinethe next control action, which must be smaller than thesampling time Ts = 0.2 s. For the specified simulations, thisthreshold was never exceeded and the average and maximumCPU times obtained in the absence of currents were 0.016 sand 0.14 s, respectively, whereas in the presence of a constantcurrent these values increased slightly to 0.023 s and 0.16 s.

The real-time implementation of the trajectory-trackingNMPC methodology for ASCs presented in this paper isalso experimentally validated in this section. The vehicleused for these sea trials is the ASC DELFIM catamaran,shown in Fig. 1, which was designed, built, and instrumentedby IST/ISR. The vehicle is equipped with a MEMSENSEnanoIMU and a global positioning system (GPS) unit work-

ing in differential mode, from which the position, attitude,and velocities of the vehicle in {I} can be computed usingwell known Kalman filtering techniques [15]. In order toenable real-time implementation of the NMPC strategy, thecontroller is implemented in a dedicated onboard computerfeaturing a Intel® Core™ 2 Duo T9550 processor, with 4GBof memory and Ubuntu 12.04 operative system with theRobotics Operative System (ROS) software framework. TheASC Delfim was tested in the Lisbon Oceanarium lake, inPortugal, using a trajectory very similar to the one used in thesimulation results and a horizon of N = 50 sampling periods.This is a harsh trajectory, with abrupt changes in velocityand direction, and is used with the objective of testing therobustness of the proposed algorithms.

The main goals of presenting these experimental resultsare twofold: the validation of the proposed NMPC strat-egy for real-time control of ASCs and the evaluation ofperformance of different configurations of the error-spacemodel. In particular, two different error-space vectors areconsidered: 1) the complete error-space vector, as definedin (II-A), which includes position integral action; and 2)the error-space vector without integral action, defined asxe =

�νTe ηT

e

�T. The results from the described sea-trials

are shown in Fig. 3, which provides the time evolution of thegeneralized position errors, actuation, and the computationtime. It can be seen that both algorithms were able tosuccessfully control the catamaran along the desired trajec-tory. The analysis of these results shows that the positiontracking error is always lower than 1.21 m, when usingintegral action, and below 0.85 m without integral action.Nonetheless, the average error is lower when using integralaction, 0.26 m, than when no integral states are considered,0.45 m. This is the result of demanding more from the NMPCcontroller, naturally yielding lower steady state errors, evenin the presence of disturbances or unmodeled dynamics. Fig.3(d) shows the CPU time that the algorithms used in eachsampling period to compute the next actuation values. Asthe CPU time is limited above by the sampling time, theimplemented algorithms are forced to abort the optimizationprocedure whenever the elapsed time values go beyond1.25Ts, maintaining the previous control value, and savingthe optimization terminal conditions conditions to enable abetter initialization value for the next sampling period. Itcan be seen that the algorithm that does not use integralaction is much faster, with a maximum CPU time of 92ms. Conversely, the integral action gets the NMPC algorithmclose to the CPU limit, as the maximum CPU time is reachedduring some transitions between trimming trajectories. How-ever, this does not compromise the operation of the vehicle.

V. CONCLUSIONS

This paper presented a NMPC strategy for motion controlof ASCs under the effect of constant currents. A nonlinearmodel of an ASC catamaran is used to define an error-spacedynamic model, which is then used by the NMPC algorithmto find the adequate control action. In contrast to the standardapproach in NMPC literature, the actuation constraints were

0 20 40 60 80 100 120 140 160

−60

−50

−40

−30

−20

−10

0

X

Y

ref

MPC

MPC int

(a) 2-D Trajectory

50 100 150 200 250 300 350

−1

−0.5

0

0.5

1

xe(t

) [m

]

50 100 150 200 250 300 350

−1

−0.5

0

0.5

1

ye(t

) [m

]

50 100 150 200 250 300 350

−0.2

0

0.2

ψe(t

) [r

ad]

Time[s]

MPC

MPC int

(b) Generalized position error, ηe

50 100 150 200 250 300 350

−4

−2

0

2

4

6

nc(t

) [r

ad/s

]

50 100 150 200 250 300 350

−2

−1

0

1

2

3

nd(t

) [r

ad/s

]

Time[s]

MPC

MPC int

(c) Actuation n

50 100 150 200 250 300 350

0.05

0.1

0.15

0.2

0.25

CP

U tim

e p

er

itera

tion [s]

Time[s]

MPC

MPC int

(d) CPU time

Fig. 3. Trajectory-tracking NMPC sea trials data.

incorporated into the model so that, while working with anerror dynamics model, every control action provided by theNMPC algorithm is always valid without affecting the overallcomputational time. The simulation and experimental resultsvalidate the real-time implementation of the proposed NMPCstrategy and show that the presented solution can effectivelysteer the vehicle along a demanding reference trajectory andin the presence of constant currents.

For some vehicles, the use of trajectory-tracking mayimpose performance bounds on the controlled system. Assome of these bounds are not present when using path-

following, and bearing in mind mission scenarios with notime-critical requirements, further modifications shall includethe formulation of a path-following error-space. Additionally,it is important that the model of the vehicle include wave dis-turbances in order to test the control algorithm performanceunder more realistic scenarios.

ACKNOWLEDGEMENTS

The authors express their gratitude to the DSOR LabDelfim team, in particular to L. Sebastião, A. Oliveira, B.Cardeira, M. Rufino, and P. Batista, having developed theDELFIM prototype ASC and for helping with the complexlogistics necessary for testing such a platform, which hasproven instrumental in the experimental validation of thetechniques presented in this paper.

REFERENCES

[1] I. Kaminer, A. Pascoal, E. Hallberg, and C. Silvestre, “Trajectorytracking for autonomous vehicles: An integrated approach to guidanceand control,” Journal of Guidance, Control, and Dynamics, vol. 21,no. 1, pp. 29–38, January 1998.

[2] C. Silvestre, A. Pascoal, and I. Kaminer, “On the design of gain-scheduled trajectory tracking contollers,” International Journal ofRobust and Nonlinear Control, vol. 12, no. 9, pp. 797–839, July 2002.

[3] W. J. Rugh and J. S. Shamma, “Research on gain scheduling,”Automatica, vol. 36, no. 10, pp. 1401–1425, October 2000, surveyPaper.

[4] N. Paulino, C. Silvestre, and R. Cunha, “Affine parameter-dependentpreview control for rotorcraft terrain following,” AIAA Journal ofGuidance, Control, and Dynamics, vol. 29, no. 6, pp. 1350–1359,2006.

[5] P. Gomes, C. Silvestre, A. Pascoal, and R. Cunha, “A coastline follow-ing preview controller for the delfimx vehicle,” in 16th InternationalOffshore and Polar Engineering Conference, Lisbon, Portugal, July2007.

[6] P. Encarnação, A. Pascoal, and M. Arcak, “Path following for au-tonomous marine craft,” in 5th IFAC Conference on Marine CraftManeuvering and Control, Aalborg, Denmark, August 2000, pp. 117–122.

[7] K. D. Do, Z. P. Jiang, and J. Pan, “Robust adaptive path following ofunderactuated ships,” Automatica, vol. 40, no. 6, pp. 929–944, June2004.

[8] D. Mayne, J. Rawlings, C. Rao, and P. Scokaert, “Constrained modelpredictive control: Stability and optimality,” Automatica, vol. 36, pp.790–814, 2000, survey Paper.

[9] G. Sutton and R. Bitmead, “Computational implementation of non-linear model predictive control to nonlinear submarine,” in NonlinearModel Predictive Control, ser. Progress in Systems and Control The-ory, F. Allgöwer and A. Zheng, Eds. Basel-Boston-Berlin: BirkhäuserVerlag, 2000, vol. 26, pp. 461–471.

[10] B. J. Guerreiro, C. Silvestre, and R. Cunha, “Terrain AvoidanceNonlinear Model Predictive Control for Autonomous Rotorcraft,”Journal of Intelligent & Robotic Systems, vol. 69, no. 1, pp. 69 –85,2012.

[11] J. Nocedal and S. Wright, Numerical Optimization, ser. Springer Seriesin Operation Reasearch. Springer, 1999.

[12] M. Prado, “Modeling and control of an autonomous oceanographicvehicle,” Master’s thesis, Instituto Superior Técnico, Lisbon, 2002.

[13] T. I. Fossen, Guidance and Control of Ocean Vehicles. New York,USA: Wiley, 1994.

[14] B. J. Guerreiro, C. Silvestre, R. Cunha, and D. Antunes, “Trajectorytracking H2 controller for autonomous helicopters: an application toindustrial chimney inspection,” in 17th IFAC Symposium on AutomaticControl in Aerospace, June 2007.

[15] J. F. Vasconcelos, B. Cardeira, C. Silvestre, P. Oliveira, and P. Batista,“Discrete-Time Complementary Filters for Attitude and Position Esti-mation: Design, Analysis and Experimental Validation,” IEEE Trans-actions on Control Systems Technology, vol. 19, pp. 181 – 198, January2011.