Nonlinear Model Predictive Control for Tracking of Underactuated Vesselsunder Input Constraints

Mohamed Abdelaal, Martin Franzle, Axel HahnComputer Science Department

University of Oldenburg, Oldenburg, Germanymohamed.abdelaal@uni-oldenburg.de, fraenzle@informatik.uni-oldenburg.de, hahn@offis.de

Abstract—In this paper, a nonlinear model predictive control(NMPC) is presented for position and velocity tracking ofunderactuated surface vessel with input constraints. A three-degree-of-freedom (3-DOF) dynamic model is used with onlytwo control variables: namely, surge force and yaw moment.Without frame transformation, a nonlinear, but convex, opti-mization problem is formulated to minimize the deviation of thevessel states from a time varying reference generated over a fi-nite horizon by a virtual vessel with the same dynamics. A real-time C-code is generated, using ACADO toolkit and qpOASESsolver, with multiple shooting technique for discretization andGauss-Newton iteration algorithm, which is computationallyefficient, thus enabling real-time implementation of proposedtechnique. MATLAB simulations is used to assess the validityof the proposed technique.

Keywords–Nonlinear model predictive control; Underactuatedvessel; Path following; Tracking

I. INTRODUCTION

Tracking control problem of surface vessels is an open re-search problem that gained a lot of interest from researchersfor many year, specially for the future expectations ofusing autonomous vessels or adding an autopilot feature forvessels. Motivated by that, surface vessels should be capableof accurate tracking techniques for following predefinedpaths at the required velocities. Underactuated vessels arethose vessels with only surge force and yaw moment, andwithout sway force, they are usually equipped with twoindependent aft thrusters or with one main aft thruster anda rudder.

Recently, there are many papers published for the trackingproblems of surface vessels, mobile robots, and also air-crafts using many different techniques. In [1], Dynamicsurface control (DSC) technique is used for global trackingof underactuated vessel in a modular way that cascadedkinematic and dynamic linearizations can be achieved. Anadaptive steering control design for uncertain ship dynamicssubject to input constraints is presented in [2]. Backsteppingwas widly used for the same problem, for instance, arecursive technique was presented in [3] for path followingof nonholonomic systems by the means of backstepping,and is demonstrated by simulation for articulated vehicleand a knife edge systems. In [4], a methodology to designstate and output feedback controller was presented by means

of Lyapunovs direct method and backstepping after modeltransformation to Serret-Frenet frame.

Various model predictive control (MPC) techniques havebeen used for the same problem. In [5], NMPC is usedfor path tracking of full actuated autonomous surface craft(ASC). In [6], model predictive control is applied for track-ing problem of underactuated surface vessels, employingthe affine property of the system model, by applying frametransformation to make the positions independent of thechoice of inertia frame, and evaluating the nonlinear func-tions using optimal states obtained at the previous instant touse linear MPC techniques. MPC for a way-point trackingof underactuated surface vessels with input constraints ispresented in [7], but the nonlinear model is linearized for theoptimization quadratic problem to be solved. Ship Headingcontrol problem is addressed in [8], [9] utilizing MPC andline of sight guidance law. In [10], an MPC technique is usedfor the nonlinear model of a 7-DOF kinematically redundantmanipulator mounted on a 6-DOF free-floating spacecraft toachieve trajectory tracking, by using feedback linearizationfor the QP problem to be formulated.

In this paper, nonlinear state feedback control law ispresented, based on solving a finite horizon optimizationproblem, at every instant, and using the current state mea-surement as initial condition for the problem. The problemtakes into consideration the physical constraints of thevessel. The output of the problem is an optimal sequence,of length N , of the vessel’s force and moment, and onlythe first element is applied and then the whole process isrepeated at the next instant. The finite horizon optimizationproblem is discretized and then formulated as a quadraticproblem (QP) which is solved by the aid of ACADO toolkit[11] and qpOASES solver [12].

This paper is organized as follows. Section II describes thevessel nonlinear dynamics, the generation of the referencetrajectory, and clarify the control objective. In section III, abrief introduction of NMPC is given with the conditionsrequired to guarantee feasibility and stability under thesolution of the optimization problem, and the implemen-tation steps done for solving it using ACADO toolkit andqpOASES solver. Simulation results done on MATLAB aregiven in Section IV. Section V concludes this paper.

Figure 1. Earth-fixed (xI , yI , zI) and body-fixed (xB , yB , zB) frames[1].

II. PROBLEM FORMULATION

The surface vessel model has 6-DOF: surge, sway, yaw,heave, roll, and pitch, which can be simplified to motion insurge, sway, and yaw under the following assumptions [1]:

1) The surge, sway, and yaw induced by wind andcurrents are negligible,

2) The inertia, added mass, and hydrodynamic dampingmatrices are diagonal,

3) The available control variables are surge force and yawmoment.

Based on that, the 3-DOF model will be [13]:

x = u cos(ψ)− υ sin(ψ)

y = u sin(ψ) + υ cos(ψ)

ψ = r

u =m2

m1υr − d1

m1u+

1

m1τu

υ =m1

m2ur − d2

m2υ

r =(m1 −m2)

m3uυ − d3

m3u+

1

m3τr.

(1)

Here, x and y are the positions, and ψ is the heading angleof the ship with respect to the earth-fixed frame, u and υ arelinear velocities and r is the angular velocity with respect tobody-fixed frame (see Figure1), the parameters m1,m2,m3

are the ship inertia including added mass effects, andd1, d2, d3 are the hydrodynamic damping coefficients. Inputconstraint is imposed to account for the maximum surgeforce and yaw moment of the ship.

A time-varying reference trajectory is generated by avirtual ship with the same dynamics as (1):

xr = ur cos(ψr)− υr sin(ψr)

yr = ur sin(ψr) + υ cos(ψr)

ψr = rr

ur =m2

m1υrrr −

d1m1

ur +1

m1τur

υr =m1

m2urrr −

d2m2

υr

rr =(m1 −m2)

m3urυr −

d3m3

ur +1

m3τrr .

(2)

where (xr, yr, ψr, ur, υr, rr) denote the reference gener-ated states. The same assumptions, as in [6], are adoptedthroughout this paper: Assumption 1: All ship state variables(position, orientation, and velocities) are measurable orcan be accurately estimated. Assumption 2: The referencevelocities and positions are smooth over time. Hence, thecontrol objective is to steer the vessel states (x, y, ψ, u, υ, r)to follow the reference states (xr, yr, ψr, ur, υr, rr) whilesatisfying the following control variables constraints:

τumin ≤ τu ≤ τumax,τrmin ≤ τr ≤ τrmax.

(3)

III. NONLINEAR MODEL PREDICTIVE CONTROL

A. Nonlinear Model Predictive Scheme

Consider a continuous time-invariant nonlinear state spacemodel of the following form:

x(t) = f(x(t), u(t)), x(0) = x0, t ≥ 0 (4)

subject to constraints

x(t) ∈ X ,u(t) ∈ U , t ≥ 0,

(5)

By discretization, we obtain:

x(k + 1) = fd(x(k), u(k)), x(j) = x0, k ≥ j (6)

subject to the constraints (5). where x ∈ <n is the statevector; u ∈ <m is the control vector; f(·, ·) is a continuousnonlinear function; f(0, 0) = 0; X ⊂ <n and U ⊂ <mare compact sets and contain the origin in their interiorpoints. In general, the scheme of NMPC and MPC is topredict the future states over finite prediction horizon using anominal model, as in (6), for the system and the last availablemeasurement or accurate estimation of the states to get theoptimum control action over a control horizon, less thanor equal the prediction one, that steer the system states tofollow a time-varying reference generated by another modelwith the same dynamics:

xr(k + 1) = fd(xr(k), ur(k)), k ≥ 0 (7)

then, the first sample of the control vector is applied and thewhole process is repeated at the next sample. Hence, NMPCis considered as a nonlinear state feedback u(k) = K(x(k))obtained online by repeated optimal control problem everysampling interval to minimize the following objective func-tion with respect to u(j):

J(x, u) =

N−1∑j=0

`(x(j), u(j)) + F (x(N)) (8)

subject to

x(j + 1) = fd(x(j), u(j)), ∗ (9a)x(j) ∈ X , ∗ (9b)u(j) ∈ U , j ∈ 0, · · · , N − 1∗ (9c)

where `(x(j), u(j)) is the stage cost function and mustsatisfy the following conditions:

• `(0, 0) = 0• `(x(j), u(j)) > 0,∀x(j) ∈ X , u(j) ∈ U , x(j) 6= xr(j),

and F (x(N), xr(N)) is the terminal cost function, and N ≥2 is length of both the prediction and control horizons.Wedefine the stage and terminal cost function:

`(x(j), u(j)) = ‖x(j)− xr(j)‖Q + ‖u(j)− ur(j)‖R (10)

F (x(N))) = ‖x(N)− xr(N)‖P (11)

where Q,R, P are positive semidefinite matrices. Controllaw is penalized because that may make optimization prob-lem easier and avoid control values of high energy [14].To guarantee asymptotic stability by using the control lawu(k) = K(x(k)), it is desirable to use infinite predictionand control horizons, i.e., set N = ∞ in (8), but it isnot applicable to get the solution of the infinite horizonnonlinear optimization problem [15]. On the other hand,stability can be guaranteed for finite horizon problems bysuitably choosing a terminal cost F and terminal attractiveregion Ω. This result has been studied in [15]–[18] andconditions required for that will be summarized as follow:

1) U ⊂ <m is compact, and X ⊂ <n is connected andcontains the orgin in the interior of U × X .

2) The vector field f : <n × <m −→ <n is locallyLipschitz in x, and satisfy f(0, 0) = 0.

3) The aforementioned conditions on ` are satisfied4) The terminal penalty F is continuous with F (0) = 0,

and the terminal attractive region satisfies Ω := x ∈X | F (x) ≤ e for some e > 0.

5) The NMPC problem has initially a feasible solution6) For any x ∈ Ω there exists a u ∈ U , such that

∂F∂x f(x, u) + `(x, u) ≤ 0.∀z ∈ ω.

Although, there are clear condition for the asymptoticstability, but designing the terminal cost F and the attractiveregion ω is still an open problem. In [14], it was shown that

Figure 2. Block digram of generating MEX files for NMPC optimizationproblem [11].

Asymptomatic stability can be guaranteed just by tuning N ,Q, and R. It was shown that closed loop stability is achievedfor relatively long horizons without the need to use terminalcost or terminal constraint [19]. Based on that, cost function(8) with ` selected as in (10) will be used in this paper andwithout a terminal constraint.

B. Implementation of the NMPC Optimization Problem

In order to solve the optimization problem (8), ACADOtoolkit is used [20] which generates highly efficient C-codefor the underlying NMPC problem. Discretization of thecontinuous time nonlinear state space model of the formx = f(x, u) is done via multiple shooting technique andthen passed to qpOASES [12] solver that is employing anactive set method. The main steps of the implementationproblem used for our problem are:

1) The continous state space model is symbolically de-fined using C-code or the ACADO MATLAB inter-face, then it is simplified employing automatic differ-entiation tools and using zero entries in the Jacobian.The result is an efficient real time C-code for theintegration of the continuous nonlinear system whichwill be used for the prediction.

2) A discretization algorithm for the problem is used toget it in the form of (6) assuming that it is inducedby sampling a continuous time system (4). Multipleshooting technique introduced in [21] is used for ourproblem and is derived from the solution of boundaryvalue problems of differential equations. The conceptof the discretization is to include some of the statesvector over the horizon x(j), j ∈ 0, · · · , N − 1 andalso not all the component of the states as additionaloptimization variables. These new variables are calledthe shooting nodes and the corresponding times willbe called the shooting times. The result is a Quadratic

x [m]-1000 0 1000 2000 3000 4000 5000 6000 7000

y[m

]

-1000

0

1000

2000

3000

4000

5000

6000

7000ActualReference

(a) Trajectory

time [sec]0 500 1000 1500 2000

u[m"s!

1 ]

5

5.1

5.2

5.3

5.4

5.5

5.6

5.7

5.8

5.9ActualReference

(b) Surge velocity

time [sec]0 500 1000 1500 2000

>[m"s!

1 ]

-0.2

-0.15

-0.1

-0.05

0

0.05ActualReference

(c) Sway velocity

time [sec]0 500 1000 1500 2000

r[r

ad"s!

1 ]

-0.005

0

0.005

0.01

0.015

0.02

0.025

0.03ActualReference

(d) Angular velocity

time [sec]0 500 1000 1500 2000

= u[N

]

#105

1.19

1.2

1.21

1.22

1.23

1.24

1.25

(e) Surge force

time [sec]0 500 1000 1500 2000

= r[N"m

]

#105

-0.5

0

0.5

1

1.5

2

2.5

(f) Yaw moment

Figure 3. Simulation results for scenario 1.

Program (QP), which is a large QP that can becondensed. See [14] and [22] for more details on theexact algorithm used.

3) The discretized problem is then passed to Gauss-Newton iterative algorithm to get a tailored algorithmfor solving the underlying QP problem.

4) Finally, qpOASES solver based on embedded variantof the active-set method is used to export C-code usingfixed dimensions and static memory.

The final product is a MEX files for both the integrator of thepredefined model, and the constrained optimization problemthat can be called from MATLAB.Figure2 summarize thesteps in a pictorial form.

IV. SIMULATION RESULTS

In this section, simulation results are presented todemonstrate the validity and asses the performance ofthe proposed NMPC for tracking of the underactuatedship (1). Simulation is done with the mex files exportedusing ACADO and qpOASES using MATLAB. These

x [m]-500 0 500 1000 1500 2000 2500 3000 3500

y[m

]

-1000

0

1000

2000

3000

4000

5000ActualReference

(a) Trajectory

time [sec]0 500 1000 1500 2000

u[m"s!

1 ]

5

5.1

5.2

5.3

5.4

5.5

5.6

5.7

5.8

5.9ActualReference

(b) Surge velocity

time [sec]0 500 1000 1500 2000

>[m"s!

1 ]

-0.2

-0.15

-0.1

-0.05

0

0.05ActualReference

(c) Sway velocity

time [sec]0 500 1000 1500 2000

r[r

ad"s!

1 ]

-0.005

0

0.005

0.01

0.015

0.02

0.025

0.03ActualReference

(d) Angular velocity

time [sec]0 500 1000 1500 2000

= u[N

]

#105

1.19

1.2

1.21

1.22

1.23

1.24

1.25

(e) Surge force

time [sec]0 500 1000 1500 2000

= r[N"m

]

#105

-0.5

0

0.5

1

1.5

2

2.5

(f) Yaw moment

Figure 4. Simulation results for scenario 2.

results have been obtained on 3.3 GHz core i5 CPU with8 GB RAM. The ship parameters are chosen as in [23] to be:m1 = 120× 103 kg d1 = 215× 102 kg ·s−1

m2 = 172.9× 103 kg d2 = 97× 103 kg ·s−1

m3 = 636× 105 kg d3 = 802× 104 kg ·m2 · s−1

Two different scenarios are presented to asses the proposedtechnique

Scenario 1: A strait line reference is generated by ap-plying τur (t) = 120.0 kN and τrr (t) = 0.0 N.m withoutexcitation for the yaw velocity as rr(t) = 0, and thefollowing initial reference conditions is used:

xr(0) = 0.0 , yr(0) = 0.0 , ψr(0) = π4

ur(0) = 5.0 , υr(0) = 0.0 , rr(0) = 0.0.

The initial condition of the ship is selected to be x(0) =[−100.0−100.00.05.00.00.0]

T . The NMPC parameters areselected to be N = 30, Q = diag(5, 5, 5, 0.2, 0.2, 0.2), andR = diag(0.001, 0.001). The constraint for the surge forceand yaw moment are

0.0 N ≤ τu ≤ 125× 103 N (12)−10.0× 108 N ·m ≤ τr ≤ 10.0× 108 N ·m. (13)

The tracked trajectory of the vessel is presented in Figure3a,and the surge, sway, and yaw velocities are depicted inFigs.3b, 3c, and 3d respectively. The surge force and yawmoments are demonstrated in Figure3e and 3f. It is shownthat the vessel can follow the straight line reference trajec-tory without the need to sway velocity (υ) excitation, whichcan not be achieved in many backstepping techniques suchas [24], while satisfying the control law bounds constraint.

Scenario 2: To assess the performance of the proposedtechnique, a curved reference path is fed to the algorithmwith reference initial condition and vessel initial conditionas in Scenario 1 and with the same NMPC parameters.

The tracking trajectory of the vessel is presented in Fig-ure4a, and the surge, sway, and yaw velocities are depictedin Figs.4b, 4c, and 4d respectively. The surge force andyaw moments are demonstrated in Figure4e and 4f. Thecontroller shows a great ability to follow curved line whilesatisfying the max surge force and yaw moment constraintsapplied.

As the vessel starts from an initial condition different fromthe reference in both scenarios, the surge speed reference isviolated until the position error is around zero, which isachieved by increasing the position factors of the weighingmatrix Q over the velocity factors. The worst-case executiontime of the generated code on the aforementioned computerare 1.4867 ms and 1.9896 ms for scenario one and tworespectively, which are extremely small compared to our10.0 seconds sampling interval and taking into considerationthe selected long prediction and control horizon of 30samples.

V. CONCLUSION

A NMPC scheme is presented for tracking of underactu-ated surface vessel, where a 3-DOF model is used with onlytwo control variables; surge force and yaw moment. Em-ploying a quadratic cost function, real-time efficient C codeis generated using ACADO toolkit and qpOASES solverwhich is called by MATLAB at each sampling interval.Simulation results show the ability of the proposed schemeto track underactuated vessels without the need to persistentexcitation of the yaw velocity, while satisfying control lawconstraints in a maximum execution time of less than 2 ms,which amounts to appr. 0.02% of the sampling interval.

ACKNOWLEDGMENT

This research is supported supported by the State of LowerSaxony as part of the project Critical Systems Engineeringfor Socio-Technical Systems (CSE). More data are avaiable:https://www.uni-oldenburg.de/en/cse/.

REFERENCES

[1] D. Chwa, “Global tracking control of underactuated ships with inputand velocity constraints using dynamic surface control method,” IEEETransactions on Control Systems Technology, vol. 19, no. 6, pp. 1357–1370, 2011.

[2] N. E. Kahveci and P. a. Ioannou, “Adaptive steering controlfor uncertain ship dynamics and stability analysis,” Automatica,vol. 49, no. 3, pp. 685–697, 2013. [Online]. Available: http://dx.doi.org/10.1016/j.automatica.2012.11.026

[3] Z. P. Jiang and H. Nijmeijer, “A recursive technique for tracking con-trol of nonholonomic systems in chained form,” IEEE Transactionson Automatic Control, vol. 44, no. 2, pp. 265–279, 1999.

[4] K. D. Do and J. Pan, “State- and output-feedback robust path-following controllers for underactuated ships using Serret-Frenetframe,” Ocean Engineering, vol. 31, no. 5-6, pp. 587–613, 2004.

[5] B. J. Guerreiro, C. Silvestre, R. Cunha, and A. Pascoal, “TrajectoryTracking Nonlinear Model Predictive Control for Autonomous Sur-face Craft,” vol. 22, no. 6, pp. 3006–3011, 2014.

[6] Z. Yan and J. Wang, “Model Predictive Control for Tracking ofUnderactuated Vessels Based on Recurrent Neural Networks,” IEEEJournal of Oceanic Engineering, vol. 37, no. 4, pp. 717–726, 2012.

[7] S.-R. Oh and J. Sun, “Path following of underactuated marinesurface vessels using line-of-sight based model predictive control,”Ocean Engineering, vol. 37, no. 2-3, pp. 289–295, 2010. [Online].Available: http://dx.doi.org/10.1016/j.oceaneng.2009.10.004

[8] A. Pavlov, H. v. Nordahl, and M. Breivik, “MPC-based optimalpath following for underactuated vessels,” IFAC Proceedings Volumes(IFAC-PapersOnline), pp. 340–345, 2009.

[9] Z. Li and J. Sun, “Disturbance compensating model predictive controlwith application to ship heading control,” IEEE Transactions onControl Systems Technology, vol. 20, no. 1, pp. 257–265, 2012.

[10] M. Wang, J. Luo, and U. Walter, “A non-linear model predictivecontroller with obstacle avoidance for a space robot,” Advances inSpace Research, 2015. [Online]. Available: http://linkinghub.elsevier.com/retrieve/pii/S0273117715004305

[11] “ACADO Toolkit.” [Online]. Available: http://www.acadotoolkit.org[12] “qpOASES Homepage.” [Online]. Available: http://www.qpoases.org[13] T. I. Fossen, Handbook of Marine Craft Hydrodynamics and Motion

Control, 2011.[14] L. Grune and J. Pannek, Nonlinear Model Predictive Control Theory

and Algorithms. Springer, 2011.[15] L. Magni, G. D. Nicolao, L. Magnani, and R. Scattolini, “A stabilizing

model-based predictive control algorithm for nonlinear systems,”Automatica, vol. 37, no. 9, pp. 1351–1362, 2001.

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

[17] C. Ong, D. Sui, and E. Gilbert, “Enlarging the terminal region ofnonlinear model predictive control using the support vector machinemethod,” Automatica, vol. 42, no. 6, pp. 1011–1016, 2006.

[18] H. Chen and F. Allgower, “A quasi-infinite horizon nonlinear modelpredictive control scheme with guaranteed stability,” Automatica,vol. 34, no. 10, pp. 1205–1217, 1998.

[19] A. Jadbabaie and J. Hauser, “On the Stability of Receding HorizonControlWith a General Terminal Cost,” IEEE TRANSACTIONS ONAUTOMATIC CONTROL, vol. 50, no. 5, pp. 674–678, 2005.

[20] B. Houska and H. Joachim, “ACADO Toolkit User’s Manual,” Sim-ulation, no. June, 2011.

[21] H. Bock and K. Plitt, “A multiple shooting algorithm for directsolution of optimal control problems,” in Proceedings of the 9th IFACWorld Congress, 1984.

[22] H. G. Bock and F. Allgower, “Real-Time Optimization for Large ScaleNonlinear Processes,” Ph.D. dissertation, University of Heidelberg,2001.

[23] K. D. Do, Z. P. Jiang, and J. Pan, “Universal controllers for stabi-lization and tracking of underactuated ships,” Systems and ControlLetters, vol. 47, no. 4, pp. 299–317, 2002.

[24] K. Y. Pettersen and H. Nijmeijer, “Underactuated ship trackingcontrol: Theory and experiments,” International Journal of Control,vol. 74, no. 14, pp. 1435–1446, 2001.