Nonlinear Model Predictive Control for Tracking of ... - uksim.info

2015 IEEE European Modelling Symposium

Nonlinear Model Predictive Control for Tracking of Underactuated Vessels under Input Constraints Mohamed Abdelaal, Martin Fr¨anzle, Axel Hahn Computer Science Department University of Oldenburg, Oldenburg, Germany [email protected], [email protected], [email protected]

of Lyapunovs direct method and backstepping after model transformation to Serret-Frenet frame. Various model predictive control (MPC) techniques have been used for the same problem. In [5], NMPC is used for path tracking of full actuated autonomous surface craft (ASC). In [6], model predictive control is applied for tracking problem of underactuated surface vessels, employing the affine property of the system model, by applying frame transformation to make the positions independent of the choice of inertia frame, and evaluating the nonlinear functions using optimal states obtained at the previous instant to use linear MPC techniques. MPC for a way-point tracking of underactuated surface vessels with input constraints is presented in [7], but the nonlinear model is linearized for the optimization quadratic problem to be solved. Ship Heading control problem is addressed in [8], [9] utilizing MPC and line of sight guidance law. In [10], an MPC technique is used for the nonlinear model of a 7-DOF kinematically redundant manipulator mounted on a 6-DOF free-floating spacecraft to achieve trajectory tracking, by using feedback linearization for the QP problem to be formulated. In this paper, nonlinear state feedback control law is presented, based on solving a finite horizon optimization problem, at every instant, and using the current state measurement as initial condition for the problem. The problem takes into consideration the physical constraints of the vessel. The output of the problem is an optimal sequence, of length N , of the vessel’s force and moment, and only the first element is applied and then the whole process is repeated at the next instant. The finite horizon optimization problem is discretized and then formulated as a quadratic problem (QP) which is solved by the aid of ACADO toolkit [11] and qpOASES solver [12]. This paper is organized as follows. Section II describes the vessel nonlinear dynamics, the generation of the reference trajectory, and clarify the control objective. In section III, a brief introduction of NMPC is given with the conditions required to guarantee feasibility and stability under the solution of the optimization problem, and the implementation steps done for solving it using ACADO toolkit and qpOASES solver. Simulation results done on MATLAB are given in Section IV. Section V concludes this paper.

Abstract—In this paper, a nonlinear model predictive control (NMPC) is presented for position and velocity tracking of underactuated surface vessel with input constraints. A threedegree-of-freedom (3-DOF) dynamic model is used with only two control variables: namely, surge force and yaw moment. Without frame transformation, a nonlinear, but convex, optimization problem is formulated to minimize the deviation of the vessel states from a time varying reference generated over a finite horizon by a virtual vessel with the same dynamics. A realtime C-code is generated, using ACADO toolkit and qpOASES solver, with multiple shooting technique for discretization and Gauss-Newton iteration algorithm, which is computationally efficient, thus enabling real-time implementation of proposed technique. MATLAB simulations is used to assess the validity of the proposed technique. Keywords–Nonlinear model predictive control; Underactuated vessel; Path following; Tracking

I. I NTRODUCTION Tracking control problem of surface vessels is an open research problem that gained a lot of interest from researchers for many year, specially for the future expectations of using autonomous vessels or adding an autopilot feature for vessels. Motivated by that, surface vessels should be capable of accurate tracking techniques for following predefined paths at the required velocities. Underactuated vessels are those vessels with only surge force and yaw moment, and without sway force, they are usually equipped with two independent aft thrusters or with one main aft thruster and a rudder. Recently, there are many papers published for the tracking problems of surface vessels, mobile robots, and also aircrafts using many different techniques. In [1], Dynamic surface control (DSC) technique is used for global tracking of underactuated vessel in a modular way that cascaded kinematic and dynamic linearizations can be achieved. An adaptive steering control design for uncertain ship dynamics subject to input constraints is presented in [2]. Backstepping was widly used for the same problem, for instance, a recursive technique was presented in [3] for path following of nonholonomic systems by the means of backstepping, and is demonstrated by simulation for articulated vehicle and a knife edge systems. In [4], a methodology to design state and output feedback controller was presented by means 978-1-5090-0206-1/15 $31.00 © 2015 IEEE DOI 10.1109/EMS.2015.85

313

A time-varying reference trajectory is generated by a virtual ship with the same dynamics as (1): x˙ r = ur cos(ψr ) − υr sin(ψr ) y˙ r = ur sin(ψr ) + υ cos(ψr ) ψ˙ r = rr m2 d1 1 u˙ r = υr rr − ur + τu m1 m1 m1 r d2 m1 u r rr − υr υ˙ r = m2 m2 (m1 − m2 ) d3 1 r˙r = u r υr − ur + τr . m3 m3 m3 r

Figure 1. [1].

(2)

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

Earth-fixed (xI , yI , zI ) and body-fixed (xB , yB , zB ) frames

II. P ROBLEM F ORMULATION The surface vessel model has 6-DOF: surge, sway, yaw, heave, roll, and pitch, which can be simplified to motion in surge, sway, and yaw under the following assumptions [1]:

τumin ≤ τu ≤ τumax , τr min ≤ τr ≤ τr max .

1) The surge, sway, and yaw induced by wind and currents are negligible, 2) The inertia, added mass, and hydrodynamic damping matrices are diagonal, 3) The available control variables are surge force and yaw moment.

(3)

III. N ONLINEAR M ODEL P REDICTIVE C ONTROL A. Nonlinear Model Predictive Scheme Consider a continuous time-invariant nonlinear state space model of the following form: x(t) ˙ = f (x(t), u(t)), x(0) = x0 , t ≥ 0

(4)

Based on that, the 3-DOF model will be [13]: subject to constraints x˙ = u cos(ψ) − υ sin(ψ)

x(t) ∈ X ,

y˙ = u sin(ψ) + υ cos(ψ) ψ˙ = r d1 1 m2 υr − u+ τu u˙ = m1 m1 m1 m1 d2 υ˙ = ur − υ m2 m2 (m1 − m2 ) d3 1 uυ − u+ τr . r˙ = m3 m3 m3

u(t) ∈ U, t ≥ 0,

(5)

By discretization, we obtain: (1)

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

(6)

subject to the constraints (5). where x ∈ ℜn is the state vector; u ∈ ℜm is the control vector; f (·, ·) is a continuous nonlinear function; f (0, 0) = 0; X ⊂ ℜn and U ⊂ ℜm are compact sets and contain the origin in their interior points. In general, the scheme of NMPC and MPC is to predict the future states over finite prediction horizon using a nominal model, as in (6), for the system and the last available measurement or accurate estimation of the states to get the optimum control action over a control horizon, less than or equal the prediction one, that steer the system states to follow a time-varying reference generated by another model with the same dynamics:

Here, x and y are the positions, and ψ is the heading angle of the ship with respect to the earth-fixed frame, u and υ are linear velocities and r is the angular velocity with respect to body-fixed frame (see Figure1), the parameters m1 , m2 , m3 are the ship inertia including added mass effects, and d1 , d2 , d3 are the hydrodynamic damping coefficients. Input constraint is imposed to account for the maximum surge force and yaw moment of the ship.

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

(7)

then, the first sample of the control vector is applied and the whole process is repeated at the next sample. Hence, NMPC is considered as a nonlinear state feedback u(k) = K(x(k)) obtained online by repeated optimal control problem every sampling interval to minimize the following objective function with respect to u(j): J(x, u) =

N −1 X

ℓ(x(j), u(j)) + F (x(N ))

(8)

j=0

subject to x(j + 1) = fd (x(j), u(j)), ∗ x(j) ∈ X,∗ u(j)

∈ U , j ∈ {0, · · · , N − 1}∗

(9a) (9b) (9c)

where ℓ(x(j), u(j)) is the stage cost function and must satisfy 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.We define the stage and terminal cost function:

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

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

ℓ(x(j), u(j)) = kx(j) − xr (j)kQ + ku(j) − ur (j)kR (10) F (x(N ))) = kx(N ) − xr (N )kP

B. Implementation of the NMPC Optimization Problem

(11)

In order to solve the optimization problem (8), ACADO toolkit is used [20] which generates highly efficient C-code for the underlying NMPC problem. Discretization of the continuous time nonlinear state space model of the form x˙ = f (x, u) is done via multiple shooting technique and then passed to qpOASES [12] solver that is employing an active set method. The main steps of the implementation problem used for our problem are: 1) The continous state space model is symbolically defined using C-code or the ACADO MATLAB interface, then it is simplified employing automatic differentiation tools and using zero entries in the Jacobian. The result is an efficient real time C-code for the integration of the continuous nonlinear system which will be used for the prediction. 2) A discretization algorithm for the problem is used to get it in the form of (6) assuming that it is induced by sampling a continuous time system (4). Multiple shooting technique introduced in [21] is used for our problem and is derived from the solution of boundary value problems of differential equations. The concept of the discretization is to include some of the states vector over the horizon x(j), j ∈ {0, · · · , N − 1} and also not all the component of the states as additional optimization variables. These new variables are called the shooting nodes and the corresponding times will be called the shooting times. The result is a Quadratic

where Q, R, P are positive semidefinite matrices. Control law is penalized because that may make optimization problem easier and avoid control values of high energy [14]. To guarantee asymptotic stability by using the control law u(k) = K(x(k)), it is desirable to use infinite prediction and control horizons, i.e., set N = ∞ in (8), but it is not applicable to get the solution of the infinite horizon nonlinear optimization problem [15]. On the other hand, stability can be guaranteed for finite horizon problems by suitably choosing a terminal cost F and terminal attractive region Ω. This result has been studied in [15]–[18] and conditions required for that will be summarized as follow: 1) U ⊂ ℜm is compact, and X ⊂ ℜn is connected and contains the orgin in the interior of U × X . 2) The vector field f : ℜn × ℜm −→ ℜn is locally Lipschitz in x, and satisfy f (0, 0) = 0. 3) The aforementioned conditions on ℓ are satisfied 4) 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 solution 6) 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 asymptotic stability, but designing the terminal cost F and the attractive region ω is still an open problem. In [14], it was shown that 315

7000

5.9 Actual Reference

6000

Actual Reference

5.8 5.7

5000

5.6




 




4000 3000 2000

5.5 5.4 5.3

1000

5.2

0

5.1

-1000 -1000

5 0

1000

2000

3000

4000

5000

6000

7000

0

500





1000

1500

2000





(a) Trajectory

(b) Surge velocity

0.05

0.03 Actual Reference

Actual Reference

0.025

0





 





 

0.02 -0.05

-0.1

0.015 0.01 0.005

-0.15 0 -0.2

-0.005 0

500

1000

1500

2000

0

500





(c) Sway velocity

2000


10

1500

2000

10

5

2.5

1.24

2

1.23

1.5












1500

(d) Angular velocity

5

1.25

1000





1.22

1

1.21

0.5

1.2

0

1.19

-0.5 0

500

1000

1500

2000

0





500

1000





(e) Surge force

(f) Yaw moment Figure 3.

Simulation results for scenario 1.

Program (QP), which is a large QP that can be condensed. See [14] and [22] for more details on the exact algorithm used. 3) The discretized problem is then passed to GaussNewton iterative algorithm to get a tailored algorithm for solving the underlying QP problem. 4) Finally, qpOASES solver based on embedded variant of the active-set method is used to export C-code using fixed dimensions and static memory.

The final product is a MEX files for both the integrator of the predefined model, and the constrained optimization problem that can be called from MATLAB.Figure2 summarize the steps in a pictorial form. IV. S IMULATION R ESULTS In this section, simulation results are presented to demonstrate the validity and asses the performance of the proposed NMPC for tracking of the underactuated ship (1). Simulation is done with the mex files exported using ACADO and qpOASES using MATLAB. These 316

5000

5.9 Actual Reference

Actual Reference

5.8

4000 5.7 5.6




 




3000

2000

1000

5.5 5.4 5.3 5.2

0 5.1 -1000 -500

5 0

500

1000

1500

2000

2500

3000

3500

0

500





1000

1500

2000





(a) Trajectory

(b) Surge velocity

0.05

0.03 Actual Reference

Actual Reference

0.025

0





 





 

0.02 -0.05

-0.1

0.015 0.01 0.005

-0.15 0 -0.2

-0.005 0

500

1000

1500

2000

0

500





(c) Sway velocity

2000


10

1500

2000

10

5

2.5

1.24

2

1.23

1.5












1500

(d) Angular velocity

5

1.25

1000





1.22

1

1.21

0.5

1.2

0

1.19

-0.5 0

500

1000

1500

2000

0

500





1000





(e) Surge force

(f) Yaw moment Figure 4.

Simulation results for scenario 2.

xr (0) = 0.0 , yr (0) = 0.0 , ψr (0) = π4 ur (0) = 5.0 , υr (0) = 0.0 , rr (0) = 0.0.

results have been obtained on 3.3 GHz core i5 CPU with 8 GB RAM. The ship parameters are chosen as in [23] to be: m1 = 120 × 103 kg d1 = 215 × 102 kg ·s−1 3 m2 = 172.9 × 10 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 proposed technique Scenario 1: A strait line reference is generated by applying τur (t) = 120.0 kN and τrr (t) = 0.0 N.m without excitation for the yaw velocity as rr (t) = 0, and the following initial reference conditions is used:

The initial condition of the ship is selected to be x(0) = T [−100.0−100.00.05.00.00.0] . The NMPC parameters are selected to be N = 30, Q = diag(5, 5, 5, 0.2, 0.2, 0.2), and R = diag(0.001, 0.001). The constraint for the surge force and yaw moment are (12) 0.0 N ≤ τu ≤ 125 × 103 N 8 8 −10.0 × 10 N · m ≤ τr ≤ 10.0 × 10 N · m. (13) 317

The tracked trajectory of the vessel is presented in Figure3a, and the surge, sway, and yaw velocities are depicted in Figs.3b, 3c, and 3d respectively. The surge force and yaw moments are demonstrated in Figure3e and 3f. It is shown that the vessel can follow the straight line reference trajectory without the need to sway velocity (υ) excitation, which can not be achieved in many backstepping techniques such as [24], while satisfying the control law bounds constraint. Scenario 2: To assess the performance of the proposed technique, a curved reference path is fed to the algorithm with reference initial condition and vessel initial condition as in Scenario 1 and with the same NMPC parameters. The tracking trajectory of the vessel is presented in Figure4a, and the surge, sway, and yaw velocities are depicted in Figs.4b, 4c, and 4d respectively. The surge force and yaw moments are demonstrated in Figure4e and 4f. The controller shows a great ability to follow curved line while satisfying the max surge force and yaw moment constraints applied. As the vessel starts from an initial condition different from the reference in both scenarios, the surge speed reference is violated until the position error is around zero, which is achieved by increasing the position factors of the weighing matrix Q over the velocity factors. The worst-case execution time of the generated code on the aforementioned computer are 1.4867 ms and 1.9896 ms for scenario one and two respectively, which are extremely small compared to our 10.0 seconds sampling interval and taking into consideration the selected long prediction and control horizon of 30 samples.

[2] N. E. Kahveci and P. a. Ioannou, “Adaptive steering control for 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 control of nonholonomic systems in chained form,” IEEE Transactions on Automatic Control, vol. 44, no. 2, pp. 265–279, 1999. [4] K. D. Do and J. Pan, “State- and output-feedback robust pathfollowing controllers for underactuated ships using Serret-Frenet frame,” Ocean Engineering, vol. 31, no. 5-6, pp. 587–613, 2004. [5] B. J. Guerreiro, C. Silvestre, R. Cunha, and A. Pascoal, “Trajectory Tracking Nonlinear Model Predictive Control for Autonomous Surface Craft,” vol. 22, no. 6, pp. 3006–3011, 2014. [6] Z. Yan and J. Wang, “Model Predictive Control for Tracking of Underactuated Vessels Based on Recurrent Neural Networks,” IEEE Journal of Oceanic Engineering, vol. 37, no. 4, pp. 717–726, 2012. [7] S.-R. Oh and J. Sun, “Path following of underactuated marine surface 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 optimal path following for underactuated vessels,” IFAC Proceedings Volumes (IFAC-PapersOnline), pp. 340–345, 2009. [9] Z. Li and J. Sun, “Disturbance compensating model predictive control with application to ship heading control,” IEEE Transactions on Control Systems Technology, vol. 20, no. 1, pp. 257–265, 2012. [10] M. Wang, J. Luo, and U. Walter, “A non-linear model predictive controller with obstacle avoidance for a space robot,” Advances in Space 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. Gr¨une 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 model predictive control: Stability and optimality,” Automatica, vol. 36, pp. 789–814, 2000. [17] C. Ong, D. Sui, and E. Gilbert, “Enlarging the terminal region of nonlinear model predictive control using the support vector machine method,” Automatica, vol. 42, no. 6, pp. 1011–1016, 2006. [18] H. Chen and F. Allg¨ower, “A quasi-infinite horizon nonlinear model predictive 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 Horizon ControlWith a General Terminal Cost,” IEEE TRANSACTIONS ON AUTOMATIC CONTROL, vol. 50, no. 5, pp. 674–678, 2005. [20] B. Houska and H. Joachim, “ACADO Toolkit User’s Manual,” Simulation, no. June, 2011. [21] H. Bock and K. Plitt, “A multiple shooting algorithm for direct solution of optimal control problems,” in Proceedings of the 9th IFAC World Congress, 1984. [22] H. G. Bock and F. Allg¨ower, “Real-Time Optimization for Large Scale Nonlinear Processes,” Ph.D. dissertation, University of Heidelberg, 2001. [23] K. D. Do, Z. P. Jiang, and J. Pan, “Universal controllers for stabilization and tracking of underactuated ships,” Systems and Control Letters, vol. 47, no. 4, pp. 299–317, 2002. [24] K. Y. Pettersen and H. Nijmeijer, “Underactuated ship tracking control: Theory and experiments,” International Journal of Control, vol. 74, no. 14, pp. 1435–1446, 2001.

V. C ONCLUSION A NMPC scheme is presented for tracking of underactuated surface vessel, where a 3-DOF model is used with only two control variables; surge force and yaw moment. Employing a quadratic cost function, real-time efficient C code is generated using ACADO toolkit and qpOASES solver which is called by MATLAB at each sampling interval. Simulation results show the ability of the proposed scheme to track underactuated vessels without the need to persistent excitation of the yaw velocity, while satisfying control law constraints 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 Lower Saxony as part of the project Critical Systems Engineering for Socio-Technical Systems (CSE). More data are avaiable: https://www.uni-oldenburg.de/en/cse/. R EFERENCES [1] D. Chwa, “Global tracking control of underactuated ships with input and velocity constraints using dynamic surface control method,” IEEE Transactions on Control Systems Technology, vol. 19, no. 6, pp. 1357– 1370, 2011.

318