Path following of a model ship using model predictive control with experimental verification

2 American Control Conference Marriott Waterfront, Baltimore, MD, USA June 3-July 2, 2 FrA5.6 Path following of a model ship using model predictive control with experimental verification Reza Ghaemi, Soryeok Oh and Jing Sun Abstract This paper presents an experimental implementation of a Model Predictive Control (MPC) strategy for path following on a model ship. The objective of the proposed algorithm is to control cross-tracking error and heading angle using the rudder control. The MPC is designed and implemented using both linearized and nonlinear model. The implementation is performed experimentally via Integrated Perturbation Analysis and Sequential Quadratic Programming (InPA-SQP) algorithm. The InPA-SQP solver can meet the computational efficiency demand and the effectiveness of the designed MPC has been verified with experimental results. I. INTRODUCTION Controlling of marine surface vessels to follow a prescribed path or track a given trajectory has been a representative control problem for marine applications and it has attracted considerable attention from the control community []-[3]. One challenge for path following problem is that the surface vessel system is under-actuated, as the path-following and heading control are often achieved using rudder angle as the only control input. Another challenge is the physical constraints imposed on the system such as rudder saturation. Model Predictive Control (MPC) is an attractive candidate to achieve zero cross tracking error and heading angle error via minimizing a suitable cost function while taking into account physical constraints. The goal of this paper is to experimentally demonstrate the effectiveness of MPC for path following of a model ship. MPC is a control technique which embeds optimization within feedback to deal with systems subject to constraints on inputs and states [4]. Over the last few decades, MPC has proven to be successful for a wide range of applications including chemical, food processing, automotive and aerospace systems [5]. Using an explicit model and the current state as the initial state to predict the future response of a plant, it determines the control action by solving a finite horizon open-loop optimal control problem on-line at each sampling interval. Furthermore, because of its natural appeal to multi-variable systems, MPC can handle underacuated problem gracefully by combining all the objectives into a single objective function. MPC has been employed for tracking control of marine surface vessels under rudder saturation constraint, [7], and Reza Ghaemi is with Department of Electrical Engineering and Computer Science at the University of Michigan, Ann Arbor, MI 489, Email: ghaemi@umich.edu. Soryeok Oh and Jing Sun are with the Department of Naval Architecture and Marine Engineering at the University of Michigan, Ann Arbor, MI 489 This work is supported in part by ONR N4-5--537. for roll motion control with fin stabilizers [8]. In [9], Tthe path-following control problem is considered where MPC is used and the performance is evaluated using simulations when the ship is subject to roll constraints and rudder saturation. This work focuses on the experimental validation of the algorithm, reported in [], for path following control. Referred to as Integrated Perturbation Analysis and Sequential Quadratic Programming (InPA-SQP), the algorithm was developed to address the computational complexity of MPC. It combines the computational advantages of perturbation analysis and optimality of the SQP solution by treating the optimization problem at time k + as a perturbed problem at time k. It is shown that this combination can significantly improve computational efficiency and is particularly useful for MPC, where an optimal control problem must be solved repeatedly over the receding horizon. This paper presents the MPC design using InPA-SQP for the path following problem for a model ship via rudder control. 3-DoF simplified nonlinear and linear models are adopted in the controller design and a corresponding 6-DoF nonlinear container model is used in simulations in order to study and compare the performance of the MPC with the linear and nonlinear model. The InPA-SQP algorithm is used to implement both the linear and nonlinear MPC on a model ship to experimentally validate the algorithm and compare the performance. II. SHIP DYNAMIC MODEL The control objective is to achieve path following for a model ship constructed in the Marine Hydrodynamic Laboratories (MHL) at the University of Michigan. The model ship, a :6 replica of a replenish vessel whose principal parameters are shown in Table I, is actuated with two contrarotating main propellers and two rudders aft. Propellers and rudders are actuated by two DC servo motors fitted with encoders and tachometers, respectively. Both the propeller speed and rudder angles are controlled by an embedded processor (PC 4) through proper mechatronic interfaces. The control code is loaded on a PC 4 through which the control signal is generated for servo motors to position the rudder accordingly. However, when the model ship is tested in the towing tank located in the Marine Hydrodynamics Laboratories (MHL) of the University of Michigan, the GPS signals are not available (even it is available, the accuracy of GPS signals is not high enough for the model test). Four infra-red cameras are used, in lieu of the GPS system, to provide the feedback signals to the control system. A picture of the instrumented model is shown in Fig.. 978--4244-7425-7//$26. 2 AACC 5236

TABLE II PARAMETERS OF THE DYNAMICS OF THE MODEL SHIP. Parameter Value m 38. I z 2.7 X u -6.8558 X u 3.2 X uu -2.959 X uuu 2.7759 Y v -7.5 Y v -2.5 Y r -.835 Y vv -24.2 Nṙ -.2 N v.965 N r -.2522 Fig.. A system overview of the fully instrumented model ship. TABLE I PARAMETERS DESCRIBING THE MODEL SHIP. Item Symbol Value Length L.6 m Breadth B.38 m Height H.7 m Mass m 38 kg Inertia I z 2.7 kgm 2 The following model characterizes the dynamics of the model ship. The procedure of model derivation and experimental validation is explained in [6]. ė = u sin(ψ) + v cos(ψ) φ = r u = m X u (mvr + X u u + X uu u u + X uuu u 3 +.892 5 U p U p ) v = m Y v ( mur + Y v v + Y r r + Y vv v v + (.298 +.435 4 35)δ) ṙ = (N v v + N r r + (.227.2 4 35)δ) I z Nṙ () with the parameters shown in Table II where U p is the propeller speed, e is the distance of the ship from the desired path, called cross tracking error, and u, v, r, and ψ are surge, sway, yaw velocities and heading angle respectively. The system is subject to saturation constraint on the rudder input δ with 3 o < δ < 3 o. (2) For way-point paths, the path following problem can be formulated as a regulation problem where the goal is steering the cross tracking error and heading angle error () to zero. Given the constraints, we employ MPC to address the regulation problem. To proceed with the MPC design, we first make the following assumption: Assumption 2.: We assume that the vessel surge velocity u is constant. Note that the propeller speed is not treated as a control variable in our path following problem formulation. Assumption 2. can be satisfied by properly controlling the propeller speed to maintain constant vessel surge speed. With Assumption 2., we can eliminate the state u and use the following simplified model for MPC design [6]. v =.29975δ.3755v.4723ur ṙ =.355δ.284r +.65475uv ė = u cos(ψ) + v sin(ψ) ψ = r, with the nominal value u =.4 m/s. The state of the system is x = [v r e ψ] T and δ is the rudder angle which acts as the only input to the system. III. PATH FOLLOWING OF THE SHIP USING MPC The MPC online optimization problem can be formulated as follows: at the time instant k, the state of the system, x(k), is observed and the following optimal control problem P N (x(k)) is solved subject to where, (3) P N (x(k)) : V N(x(k)) = min u {V N (x(k), u)} (4) V N (x(k), u) = k+n j=k L(x(j), u(j)) + Φ(x(N)) (5) x(j + ) = f(x(j), u(j)), f : R n+m R n ; (6) x() = x(k) R n ; (7) C(x( ), u( )), C : R n+m R l. (8) u = {u(k), u(k + ),...,u(k + N )}, (9) is the control sequence, x(j) := x u (j;x(k)), j = k, k +,, k + N () 5237

is the state trajectory at time instant j resulting from an initial state x(k) and the control sequence u, and Φ(x(N)) is the penalty for the final states. For the ship path following problem where the objective is to regulate the cross track and heading angle to zero, the dynamic equation (3) and inequality constraint (2) are used in the place of (6) and (8) respectively and the cost (5) is defined as L(x(k), u(k)) = x(k) T Qx(k) + u(k) T Ru(k), Φ(x(N)) = αx(n) T S f x(n), Q = diag([ 8]), R =, α =,. () S f is the solution of the matrix Ricatti equation associated with R and Q. For MPC optimization, one can either use the nonlinear model (3), which leads to a non-linear MPC (NLMPC), or a linearized version of (3) which results in linear MPC (LMPC). In this work, we consider both LMPC and NLMPC and analyze their performance for ship path following under the given implementation hardware. Remark 3.: While stability analysis is important in design of MPC [4], the analysis is not within the scop of the paper. Consider the initial state of the system () as x = [2,,.5,,] T. To compare the linear and nonlinear model, we solve the optimization problem P(x ) with N = 2 and sampling time., for both linear and nonlinear systems. Figure 2 shows the resulting open loop optimal control command for linear and nonlinear systems. Rudder angle (rad).6.4.2.2.4.6 Optimal control command with linear model Optimal control command with nonlinear model.8 2 4 6 8 2 4 6 8 2 samples(.s sampling rate) Fig. 2. Open loop optimal control commands determined by nonlinear and linear MPC It is shown that the optimal solution for both systems are almost identical. When used in the MPC feedback control, the response of the closed-loop LMPC and NLMPC are shown in Figures 3 and 4 for the cross tracking error and rudder command respectively. Note that both LMPC and NLMPC are applied to the nonlinear model () in our simulations. It can be seen that while the performance of the two controllers are the same, the control commands for the nonlinear and linear MPC are not similar. Accumulation of the small difference, shown in Figure 2, over the horizon is the cause of such difference. Besides the performance, computational demand is another important consideration in deciding which class of MPC is cross tracking error (m) 2.5 2.5.5 Fig. 3. Rudder angle command (deg) Nonlinear cross tracking error Linear cross tracking error 5 5 2 25 3 time(s) 3 2 2 The resulting cross tracking error generated by MPC linear MPC Nonlinear MPC 3 5 5 2 25 3 time(s) Fig. 4. Control command generated by MPC more appropriate for our application. With the InPA-SQP algorithm, which is elaborated in the next section, we evaluated both LMPC and NLMPC on a real-time simulator with a sampling time of. sec. We note that both LMPC and NLMPC are computationally feasible without any overrun. IV. INPA-SQP ALGORITHM To make the MPC scheme feasible for practical implementation, we have developed a computationally efficient method for MPC optimization []. The method is briefly introduced, which is referred to as the Integrated Perturbation Analysis and Sequential Quadratic Programming (InPA-SQP) approach [], [], for the implicit MPC implementation. According to the MPC strategy, at the time instant k, the state of the system, x(k), is observed and the optimal control problem P N (x(k)) defined by (4) (8) is solved. The resulting optimal control sequence is u (x(k)) = {u (;x(k)), u (;x(k)),...,u (N ;x(k))}, (2) the optimal state trajectory is x (x(k))={x (;x(k)):=x, x (;x(k)),...,x (N;x(k))}, (3) and the model predictive control law is h(x(k)) := u (;x(k)). (4) According to InPA-SQP approach, the optimal solution at each time instant k for the observed state x(k), is calculated 5238

using the optimal solution at the previous time instant, i.e., k. Given u (x(k )), the optimal control sequence u (x(k)) can be computed using perturbation analysis where the initial state for the optimization problem P N (x(k )) is perturbed by x(k) = x(k) x(k ). If the variation x(k) in the initial state of the optimization problem P N (x(k )) causes changes in the activity status of constraints, the variation is split into small variations, i.e., x i (k), i =,2, (with intermediate points x i (k)), where by each variation one constraint changes from active to inactive or vise versa. With these small variations, some intermediate points are identified along the line connecting x(k ) and x(k). Then the corresponding approximation of the optimal control sequence u (x i (k)) are calculated using perturbation analysis. Fig.5 illustrates intermediate initial conditions which handle the large perturbation, where the large perturbation is decomposed into smaller intermediate ones of α δx () (k), α δx () (k), etc. Fig. 5. Intermediate initial conditions which handle the large perturbation. Note that the control sequence provided by perturbation analysis at each intermediate point x i (k), i.e., u (x i (k)) is an approximation to the optimal control sequence. Therefore, the error can accumulate at several steps of perturbational analysis to be performed at those intermediate steps. To compensate this error and maintain the optimality of the approximate solution, we use a special formulation of SQP with active set method to modify the algorithm corresponding to the perturbation analysis approach. This special formulation allows us to cast the solution of the SQP into a formula which is similar to that of the perturbation analysis, thereby facilitating their seamless integration to speed up the calculation of the optimal solution. The detailed algorithm is presented as follows: prediction time instant k + j as H(k + j) := L(x(k + j), u(k + j)) + λ(k + j + ) T f(x(k + j), u(k + j)) + µ(k + j) T C(x(k + j), u(k + j)), j =,, N (5) where, µ(k + j) is the Lagrange multiplier associated with the inequality constraint (8) and λ(k+j+) is the Lagrange multiplier associated with equality constraint (6) at the prediction time instant k + j. If there is a perturbation δx in the initial state x, we go through the following steps to calculate the optimal solution for the initial state x + δx (for MPC, at each iteration, we have x = x(k), x + δx = x(k) + x(k)): ) Update x(k) either with state measurement or state observation, while assuming u (x(k )) and x(k ) are given from previous time instant. 2) Set i =, δx () (k) = x(k) x(k ), x (k + j) = x (k + j;x ), and u (k + j) = u (k + j;x ). 3) Calculate the state and control variation δx i (k + j) and δu i (k + j) with respect to x i (k + j), u i (k + j) for the initial state perturbation δx (i) (k), j [, N] as follows: δu(k + j)= [I ]K (k+j) [ ] Zux (k+j)δx(k+j)+f u(k+j)t(k+j+)+h T u (k+j) Cx(k+j)δx(k+j) a, δx(k+j+)=f x (k+j)δx(k+j)+f u (k+j)δu(k+j), δx(k) = δx (i) (k), (6) where H u (k + j) is the partial derivative of the Hamiltonian function with respect to control u at the prediction time instant k + j, S(k + N) = Φ xx (k + N), S(k + j) = Z xx (k + j) [ [Z xu (k + j) C xt a Zux (k + j) (k + j)]k (k + j) Cx(k a + j) T(k + N) =, T(k+j)=f T x (k+j)t(k+j +) [Z xu (k+j) C xt a (k+j)]k (k+j) [ ] f T u (k+j)t(k+j +)+H u (k+j), ], (7) Z uu (k+j)=h uu (k+j)+f T u (k+j)s(k+j+)f u (k+j), Z ux (k+j)=h ux (k+j)+f T u (k+j)s(k+j+)f x (k+j), Z xx (k+j)=h xx (k+j)+f T x (k+j)s(k+j+)f x (k+j), Z xu (k+j)=z ux (k+j) T (8) Assume that functions L(x( ), u( )), L : R n+m R, Φ(x( )), Φ : R n+m R, f(x( ), u( )), C(x( ), u( )) are twice continuously differentiable with respect to x and u. Moreover, let us define the Hamiltonian function H at each 5239 if the constraint is active, [ Zuu (k + j) C K (k+j) = u at (k + j) Cu(k a + j) ], (9)

where C a (k + j) is a vector consisting of those elements of the vector C(x(k + j), u(k + j)) whose corresponding inequality constraints are active. If no constraint is active at the prediction time instant k+j, K (k + j) = [Z uu (k + j)], j =,, N. (2) Moreover, when the constraint is active, calculate the optimal Lagrange multiplier variation [δµ i (k), δµ i (k+ ), δµ i (k + N )] corresponding to initial state variation δ i x(k) when the constraint is active as follows: δµ i (k + j) = [ I]K (k + j) [ ] Zux (k + j)δx(k + j)+fu T (k)t(k + j + )+H u (k + j) Cx(k a. + j)δx(k + j) (2) All matrices are evaluated at x i ( ) and u i ( ). 4) Find the smallest α ik [,] such that the perturbation α ik δx (i) (k) changes the status of the constraint at least at one instant, namely: α i = min k {α ik, k =,, N }. If α i < or α i >, set α i =. 5) If α i, set and terminate. Otherwise: u i+ ( ) = u i ( ) + δu i ( ) x i+ ( ) = x i ( ) + δx i ( ) (22) If α i =, change the activity status of the corresponding constraint accordingly. That is, if α i corresponds an active (inactive) constraint, set the constraint inactive (active). Go to step 2. If α i < set and u i+ ( ) = u i ( ) + α i δu i ( ) x i+ ( ) = x i ( ) + α i δx i ( ) δx (i+) (k) = ( α i )δx (i) (k), x (i+) (k) = x (i) (k) + α i δx (i) (k), i = i +. (23) Go to step 2. Fig.6 shows the flowchart of the InPA-SQP algorithm. Note that the intermediate perturbed initial conditions lie on the line connecting x to x + δx. The perturbed optimal control solution corresponding to the large perturbation δx can be approximated by augmenting the nominal solution with the intermediate ones as: u (k + j;x ) + i δu i (k + j). If at the point x +δx, H u (k) is not small enough, the prescribed procedure goes on with zero initial state perturbation δx i = until the optimal solution with H u (k), k [, N] is achieved. 524 (i) Seti, x (k) x(k) x(k ), i * i * x(j) x (j;x(k)), u(j) u (j;x(k)), j k: k N x (i) (k) i i (i) u ( ) u ( ) i u ( ) xi ( ) xi( ) x(i) i ( ) (i ) (i) x (k) ( i) x (k) xi (k) xi(k) x(i) i (k) i i Fig. 6. x (i) (j) u (i) (j) x i (j) u i (j) [,] ik x (i) ik (k) i i i (i) u () u() u () Hu( ) x (i) (k) i i Flowchart of the InPA-SQP algorithm. The InPA-SQP synergistically combines the solutions derived using perturbation analysis and SQP to solve the optimization problem with mixed input and state constraints. It is shown in [] that it can significantly improve the computational efficiency while effectively handling the nonlinear constraints, making the MPC feasible for systems with fast dynamics. With the introduction of the InPA-SQP solver, we now proceed to implement the MPC on our prototype system. V. EXPERIMENTAL PLATFORM AND EXPERIMENTAL RESULTS For real-time implementation, feedback control is accomplished using a PC-based PC4 hardware which runs the QNX real-time operating system. PC4 communicates, through a wireless LAN, to a host PC, on which the control algorithm is programmed and tuned, data acquisition function is performed. Ship position signals are collected from the camera system and transmitted to PC4 via RF modems. The key control and communication devices are shown in Fig. 7, together with the connections among the devices. To implement MPC with a nonlinear model, the parameters R and α in () are tuned to R =.5 and α =.5 and for MPC with linear model, the parameters are tuned to R =.8 and α =.5. Figures 8 and 9 show the experimental results of implementing linear and non-linear MPC on the ship with sampling rate. sec. Y pos represents the position of the ship on the Y-axis. As expected, the results observed in simulations are achieved with the same MPC parameters, except that now oscillations in control command are observed. The observed oscillation can be attributed to delays incurred in the wireless communication and in the computation of the position of the ship from signals provided by cameras which is a subject of future studies.

Fig. 7. Wireless link between devices. Fig. 8. Response of LMPC with N = 7 Fig. 9. Response of NLMPC with N = 7 VI. CONCLUSION In this paper, a Model Predictive Controller is designed for path following of a model ship subject to input constraints. To implement MPC with a nonlinear model, the InPA-SQP method is introduced to solve the constrained optimal control problem. The InPA-SQP solver can meet the computational efficiency demand even for the nonlinear model considered in this paper. The effectiveness of the proposed MPC has been verified with experimental results. REFERENCES [] T. Fossen, M. Breivik, and R. Skjetne, Line-of-sight path following of underactuated marine craft, Proceeding of the Sixth IFAC Conference Maneuvering and Control of Marine Crafts, pp. 244-249, 23. [2] Z. Li, J. Sun, and S. Oh, A robust nonlinear control design for path following of a marine surface vessel, Proceedings of IFAC Conference on Control Applications in Marine Systems, 27. [3] R. Skjetne, T. I. Fossen, and P. Kokotovic, Robust output maneuvering for a class of nonlinear systems, Automatica, vol. 4, pp. 373-383, 24. [4] D. Q. Mayne, J. B. Rawlings, C. V. Rao, P. O. M. Scokaert, Constrained model predictive control: stability and optimality, Automatica, vol. 36, pp. 789-84, 2. [5] S. Qin and T. Badgwell, A survey of industrial model predictive control technology, Control engineering practice, vol., pp. 733-764, 23. [6] S. Oh, J. Sun, Z. Li, E. A. Celkis, and D. Parsons, System Identification of a Model Ship Using a Mechatronic System, [7] A. Wahl and E. Gilles, Track-keeping on waterways using model predictive control, Proceedings of Control Applications in Marine Systems, 998. [8] T. Perez and G. C. Goodwin, Constrained predictive control of ship fin stabilizers to prevent dynamic stall, Control Engineering Practice, vol. 6, pp. 482-494, 28. [9] Zhen Li, Jing Sun and Soryeok Oh, Path Following for Marine Surface Vessels with Rudder and Roll Constraints: an MPC Approach, Proceedings of American Control Conference, 29. [] R. Ghaemi, J. Sun, and I. Kolmanovsky, An Integrated Perturbation Analysis and Sequential Quadratic Programming Approach for Model Predictive Control, Automatica, Vol. 45, pp. 242-248, 29. [] R. Ghaemi, J. Sun, and I. Kolmanovsky, Neighboring Extremal Solution for Nonlinear Discrete-Time Optimal Control Problems with State Inequality Constraints, IEEE Transaction on Automatic Control, Vol. 54, No., 29. 524