AModel-Based Decoupling Method for Surge Speed and Heading Control in Vessel Path Following

Guangdong HUST Industrial Technology Research Institute, Guangdong Province Key Laboratory of Digital Manufacturing Equipment, Key Laboratory of Image Processing and Intelligent Control of the Ministry of Education of China, School of Artificial Intelligence and Automation, Huazhong University of Science and Technology, Wuhan 430074, China Guangdong HUST Industrial Technology Research Institute, Dongguan 523000, Guangdong, China


Introduction
Unmanned Aerial Vehicle (UAV), Unmanned Ground Vehicle (UGV), and Unmanned surface vessel (USV) have attracted more and more attention in recent years. e unmanned agents have great potentials in applications such as patrol, hydrologic exploration, and transportation when they are cooperative as multiagent systems. e coordinated control for multiagent systems has received increasing attention recently [1][2][3][4][5][6], also the control of single agent is essential to the whole multiagent system. Path following is a very crucial technology for Unmanned surface vessels (USVs) sailing in the ocean; many applications require that the USVs have the capability to drive to the task area along a predefined path. e purpose of the path following controller is to minimize the error between the real position of USVs and the predefined path in vessel sailing. e path guidance laws designed at the kinematic level can be used for way-point path following [7][8][9] and curve path following [10][11][12][13][14]. For example, autopilot is a kind of path-following controller at the kinematic level which has been wildly used in ocean transportation.
Many other methods which combine the kinematic model and kinetics model are also proposed to achieve the goal of path following. Some robust nonlinear methods are used to force the vessel to follow the predefined path. In [15], Lyapunov's direct approach and backstepping method are used to design the controller. In [16], several coordinate transformations and the backstepping technique are used to design a global controller for vessel path following. In [17], the backstepping nonlinear controller design is based on feedback dominance, and the experiment results demonstrate the effectiveness of the proposed method. In [18], the path following controller design is divided into two parts via backstepping technique. In [19], a backstepping-based pathfollowing control algorithm is presented to ensure that the tracking errors of the surface vessel remain within the required performance constraints. In [20], a neural sliding mode controller is designed for vessel path following in the external disturbance and parameter perturbation. In [21,22], the stability of integral line-of-sight (ILOS) guidance method for a path-following system of underactuated marine vehicles is analyzed. In order to reduce the large cross-track error in turning by adjusting vessel speed, an inverse optimal control method is proposed in [23]. In [24], a reference governor is used for generating the optimal reference signals within the state and input constraints, and the optimization problem is solved by a projection neural network.
It is a challenge to address the actuator saturation and state constraints in controller design for robust nonlinear methods. In [25], the error constraint of the vessel position is handled by integrating a novel tan-type barrier Lyapunov function. However, how to handle lots of different actuators and state constraints is still difficult in the robust method. Due to the advantage of dealing with constraints, many methods under the framework of MPC are proposed to solve path-following problems. In [17], the path-following problem is solved by using a linear MPC controller with rudder and roll constraints. In [26], an LOS decision variable can be incorporated into the MPC design. e lookahead distance is optimized in solving the MPC problem so that the path following has a better performance than the constant lookahead distance LOS method. In [27], a nonlinear statespace model consists of a path following error model and a kinetic model as the tracking model is established; then, a nonlinear MPC controller is designed. In [28], a robust MPC method is proposed for vessel path following control under the constraints of the rudder angle and roll angle. In [29], a nonlinear MPC controller is designed to solve the combined path planning and tracking control problem for an AUV.
In the application of the way-point path following, surge speed control is essential since time constraint is a condition that cannot be ignored. In [7], the speed is controlled through the state feedback linearization method. In [30], different approaches such as proportional-integral and feedback linearization are tested in vessel speed control experiments. Except for designing a speed controller independently, the speed and yaw subsystem can be designed together. In [31], a dynamic window-based controller is used to control the surge speed under the actuator constraints. In [32], the speed and yaw subsystem are controlled through the backstepping and Lyapunov method.
In this paper, to improve the speed and heading controller performance in path following, a model-based decoupling (MBD) method is proposed. e heading controller is designed first under the framework of MPC; then, the coupling terms between the surge model and yaw subsystem are predicted through the output of heading controller. In the speed MPC controller, the coupling terms are compensated as a time-varying disturbance. e speed and heading control of the vessel using a kinetic model is a nonlinear control problem under the framework of MPC. By using the MBD method, the original nonlinear MPC control problem breaks down into two low-dimensional nonlinear MPC problem or two linear MPC problem. e organization of this paper is as follows. Section 2 states the problem formulation. Section 3 presents the design of the path following guidance laws and MBD method. Section 4 illustrates and analyzes the simulation results. Section 5 summarizes conclusions.

Mathematical Model for Vessel Path Following.
In this section, the kinematic and kinetic model of USVs is introduced first. e vessel model normally is a six degree-offreedom (DOF) nonlinear model, including surge, lateral, vertical velocity, and pitch, roll, and yaw angle. e states in 6-DOF model such as pitch angle and roll angle can be neglected in path-following control, so, the kinematic model in north-east coordinate such as equation (1a) and the 3-DOF kinetic model proposed in [33] such as equation (1b), are used here to describe path following: where the vector η � x y ψ T denotes the position and yaw angle in the north-east coordinate and R(η) is the rotation matrix. e detail form of equation (1a) is where ] � u v r T denotes the surge velocity, lateral velocity, and yaw angle velocity in the body-fixed coordinate. Vector τ � τx 0 τz T denotes actuator force and torques, τ wind denotes force and torques caused by wind, τ wave denotes force and torques caused by the wave, M accounts for inertial effects, matrix C(]) accounts for centrifugal and Coriolis effects, and matrix D(]) accounts for viscous and dissipative effects. e exact expression of the matrix is where 2 Mathematical Problems in Engineering e vessel named CyberShip II in [33] has two main propellers, two rudders aft, and one bow thruster fore. For simple but without loss of generality, the vessel model of CyberShip II used in this paper has no bow thruster, but one propeller and one rudder. e surging force generated by one propeller is equal to two propellers of CyberShip II, and so is the steering moment. e exact expression of actuator configuration matrix B is e kinetic model can be rewritten as _

Path-following Formulation.
e guidance law used in this paper is designed at the kinematic level. Usually, the C 1 parametrized path (x p (θ), y p (θ)) with θ ≥ 0 are used to denote path including curve path or way-point path. In this paper, the cross-error of the path following is our concern, and the expected surge speed is given by manual. Since many path-following guidance laws are valid in both curve path following and way-point path following, the situation of the way-point path following is only considered in this paper for simplicity. For a vessel located at (x, y), the orthogonal distance y e to the path can be calculated by equation (7) proposed in [10]: where θ is the slop of straight line between two way-point . e cross point (x p , y p ) can be calculated by the following equation.
when θ � 0, cross point (x p , y p ) � (x, y i+1 ), and when θ � π/2, cross point (x p , y p )�(x i+1 , y). Equation (7) is differentiable, and the differential form is Equation (9) can be written in a more compact form as where U � is the speed sideslip angle. In this paper, all the states including sideslip assumption can be measured directly. Assuming that sideslip angle β is small and constant implies cos(β) ≈ 1 and sin(β) ≈ β. Equation (10) can be simplified as

MBD Method for Surge Speed and Heading Control
In this section, the path-following guidance law is introduced first, since the focus of this paper is the MBD method, and the path-following guidance law equation (12) proposed in [10] is used in this paper to generated reference heading ψ d ; the stability of the guidance laws can be referred to [10] Robust nonlinear methods such as the backstepping method and direct Lyapunov method can be used to design surge speed and heading controller based on the nonlinear 3-DOF maneuvering model. However, these methods cannot handle the constraints of state and actuator. Also, it is a challenge to design such a controller when the maneuvering model is complex. e nonlinear MPC method based on the 3-DOF maneuvering model can be used to control heading and surge speed together under the constraints [34]; the solution of the nonlinear MPC problem can only be solved through numerical iteration methods such as Gauss-Newton method. All solutions based on gradient iteration method can only guarantee the local minimum, also it is a huge computation burden for on-board computer in real-time control.
To reduce the complexity of designing controller, usually, the surge speed controller and heading controller are designed independently due to which the 3-DOF maneuvering model can be decoupled in a forward speed (surge) model and a sway-yaw subsystem for maneuvering [35]. e independent surge speed and heading controller both have good performance when the reference heading and reference speed are steady. However, there is an inevitable speed loss when there is sway-yaw motion. e MBD method for surge speed and heading control are designed under the framework of MPC. Figure 1 shows the guidance and control architecture for vessel path following. e MPC heading controller is designed independently based on the sway-yaw model linearized at a given work point. e MPC surge speed controller is also based on the linearized surge model. e time-varying added resistance generated by sway-yaw motion is only considered in the process of solving the optimal sequence, which does not add much computation burden. e nonlinear MPC problem breaks down into two low-dimensional linear MPC problem through the MBD method. Since the heading controller and the surge speed controller are designed independently, it is much more convenient for parameters turning in the controller design process.
During every MBD method control interval, the MPC heading controller is solved fist. en, the coupling terms of the surge model are calculated through the outcome sequence of the MPC heading controller, and these coupling terms are treated as time-varying added resistance. Next, the MPC surge model is solved considering the time-varying added resistance. Finally, the first element of the heading controller outcome sequence and the speed controller outcome sequence are used as controller output. e rest of this section now focuses on the derivation of the MBD method.

Heading Controller.
In this section, the MPC heading controller with actuator constrains is first designed. e purpose of the heading controller is to steer the vessel heading to the excepted heading ψ d .
During the path-following task, the surge speed is controlled by a speed controller, so we assume that the surge speed is constant during every control interval, and the sway-yaw subsystem equations (6b) and (6c) can be decoupled from an original nonlinear system. e linear state-space model is derived by linearizing the sway-yaw subsystem at a given work point to reduce the complexity of designing the controller. Assuming surge speed u � u o , lateral velocity v ≈ 0, yaw angular velocity r ≈ 0, and the right hand of equations (6b) and (6c) can express in a Taylor series by taking the partial derivative. By retaining the firstorder terms of Taylor series and incorporating equation with _ ψ � r, the linearized sway-yaw subsystem can be written as where e controllers are designed in a discrete form; the heading model can be simplified to a first-order transfer function model. Normally, the sampling interval T s is chosen as one-tenth of the time constant in the simplified first-order function model. When the sampling interval T s is given, the discreet form of equation (13) can be easily calculated by the following transform: where Due to the effect of wind, current, wave, and unknown disturbance, the predicted progress incorporating the external disturbance d is used to correct the prediction results at every control interval. For the simplicity of solving online optimization, equation (14) is written in the augmented form as C � 0 0 1 0 , and I is the identity matrix.
As we all know, the energy output by the physical system is not infinite, and the torque force generated by vessel actuators such as a rudder or water-jet system has the torque rate constraints and bounded value constraints. So, the constraints of the actuator are considered in the heading control online optimal progress, Δ τz and Δτz are the lower and upper bounds of the τz varying rate, respectively, and τz and τz are the lower and upper bounds of the τz value, respectively. e cost function consists of two quadratic terms, the heading error quadratic term grantee that the real heading convergence to the reference heading and the rudder varying rate quadratic term can reduce the rudder movement. en, the heading control problem is transformed into a constrained online optimization problem as equation (16), N p is the prediction horizon, and N u is the control horizon: which subjects to e online optimization problem equation (16) can be transformed into a quadratic problem to solve the optimal solution sequence Δτz * � Δτz * k , Δτz * k+2 , . . . , Δτz * k+N u − 1 , at each time k. According to equations (15a) and (15b), the state prediction with prediction horizon N p and control horizon N u can be written as where disturbance at time k is updated by the error of real state and predicted state as Substituting equation (18) into equation (16) and ignoring the constant value terms which do not affect the solution of solving the optimization problem, the cost function can be written as a quadratic form: Matrix S � H T QH + R is the quadratic objective term, and matrix f � H T Q(Pz k + Ed k − ψ d ) is the linear term. e constrained quadratic programming can be solved by the numerical method. e controller output τz k � τz k− 1 + Δτz k , and Δτz k is the first element of Δτz * . e stability of the MPC controller without terminal constraints can be guaranteed by choosing a large prediction horizon [36].

Surge Speed Controller.
In this section, the MPC surge speed controller with surge force constraints is designed. Under the framework of MPC, the constraints of the state and actuator can be handled easily. e standard MPC (SMPC) method is first introduced to the design surge speed controller; then, the surge speed controller based on the MBD method is introduced.
e discredited linear surge model equation (21) can be derived by linearizing the nonlinear surge model equation (6a) at a given work point: where A 1 � e a 1 T s and B 1 � T s 0 e a 1 T s dta 7 . When a vessel is in the surge motion, yawing motion due to steering can result in added resistance in calm water as well as yawing due to wave motion [37], and the added resistance can significantly reduce surge speed. Usually, the surge speed controller is designed independently based on the linear surge model equation (21) through the standard MPC method and all the disturbance, including the added resistance and the effect of the wave, wind, and current are treated as an unknown disturbance. e derivation of the SMPC method is introduced as follows. Considering the unknown disturbance ω and rewriting the sure model equation (21) as augmented for simplicity, the modified surge model is is updated by the error of real surge speed and predicted surge speed as en, the augmented surge model is used to construct a MPC controller.
Based on the modified surge model, the surge speed control problem with excepted surge speed u d now is an online optimization problem as cost function equation (24). e speed error quadratic term in equation (24) grantee that the real speed convergence to the reference speed and the force varying rate quadratic term in equation (24) can reduce the force varying. e first element of optimal solution sequence Δτx * � Δτx * k , Δτx * k+2 , . . . , Δτx * k+N u − 1 , is the surge force increment at each time k. e prediction horizon is N pu , and control horizon is N uu : which subjects to τx ≤ τx k+j ≤ τx, j � 1, 2, . . . , N uu .
e drawback of the SMPC is that there will be no compensating force of disturbance until there is an offset in Mathematical Problems in Engineering surge speed since the process of updating disturbance ω at time k always occurs at the next control interval. e MBD method is proposed in this paper to overcome the delay in updating disturbance. In the MBD method, the sway-yaw motion due to the steering is predicted by the actuator sequence Δτz * and compensated as a time-varying disturbance pc � pc k , pc k+1 , . . . , pc k+N pd − 1 in forward speed control. e calculation of pc involves several steps, as described in the sequel: (1) At time k, calculate the predicted sequence of v, r through equations (15a) and (15b) by using Δτz * . (2) Calculate the time-varying disturbance pc k by using part terms of equation (6a), pc k � a 4 v k r k + a 5 r 2 k . e states v k and r k are measured directly at present time k.
Substituting pc into equations (22a) and (22b), the surge model is modified as follows: and the equation of updating ω k is modified as e cost function based on equations (26a) and (26b) and equation (27) can be written as which subjects to Also, the online optimization problem equations (25a), (25b), and (28) can be transformed into constrained quadratic programming, the controller output τx k � τx k− 1 + Δτx k , and Δτx k is the first element of Δτx * . e stability of the MPC controller can be guaranteed by choosing a large prediction horizon N pu . Since the focus of this paper is the performance of the MBD method, the detailed proof of MPC stability can be referred to [36].

Simulation Results
In this section, the results of the way-point path following are presented. e simulation results of the MBD method and the standard MPC (SMPC) method are compared to illustrate the superiority of the MBD method. en, a time-varying disturbance incorporating white noise and constant offset force is added in path-following simulations to demonstrate the effectiveness and robustness of the MBD method. e parameters of the vessel are given in [33] and listed in Table 1.
In the simulation, the parameter Δ in guidance law is chosen as Δ � 8 m. In the heading MPC Controller, the prediction horizon N p and control horizon N u are chosen as 70 and 30, respectively. e matrix Q h is an identity matrix with dimension N p × N p , the matrix R h is also an identity matrix with dimension N u × N u . In the surge speed controller, the prediction horizon N pu and control horizon N uu are chosen as 30 and 5, respectively. e matrix Q u is an identity matrix with dimension N pu × N pu ; the matrix R u is also an identity matrix with dimension N uu × N uu . In the surge speed controller, the vessel initial forward speed of the vessel is set at 0.5 m/s and the other state of vessel are all set to zero. e constraints of surge force are |τx| ≤ 5. e constraints of steering moment are |τz| ≤ 1, and the steering moment increment is |Δτz| ≤ 0.5. e path-following results of different methods with no external disturbance are shown in Figure 2, and the corresponding surge speed and surge force τx are shown in Figure 3. From the results shown in Figure 2, we can see that the path following results of different methods are very close and the actual path of the vessel converges to the reference path eventually.
In the surge speed control, as shown in Figure 3, the commanded speed is 0.8 m/s at the first 200 s then changed to 0.6 m/s. All the surge control method can guarantee the surge speed converge to the command speed. Due to the way-point changed, the sway and yawing motion result in added resistance. ere is some speed loss in path following x-coordinate m  Figure 3(b)), we can see, comparing the surge force output by the MBD method and the SMPC method, that the surge force output by SMPC controller has a time delay when there is an add resistance. Moreover, the time delay of surge force is the reason why there is a significant surge speed loss in speed control.
In the MBD method, the sway and yawing motion generated by the steering moment is predicted as a time-varying disturbance, which is compensated at the speed control. Nevertheless, in SMPC control, there will be no external force to compensate disturbance until there is an offset in surge speed. So, the MBD method has a better performance than the SMPC method in heading and speed control of the vessel in path following.
To further demonstrate the effectiveness of the MBD method in the real sea environment, the path following results with time-varying disturbance is shown in    From Figure 4, we can see that the cross error Y e in path following converges to zero; the addition unknown timevarying disturbance has no influence in the overall performance. e heading and speed control results shown in Figures 5 and 6 demonstrate the effectiveness of the MBD method under unknown time-varying disturbance. Also, the steering moment τz, the increment of steering moment Δτz, and the surge force τx are all restricted to the constraints.

Conclusions
In this paper, to solve the surge speed loss in path following due to the added resistance generated by sway-yaw motion, the MBD method for surge speed and heading control in vessel path following is presented. In the MBD method, the heading controller and the surge speed controller can be designed under the framework of MPC, independently. e nonlinear control problem of control vessel heading and controller together can be decoupled into two MPC control problems based on the linear heading model and linear surge model, which makes the progress of designing the controller easier. e output of the heading controller is used to predict the time-varying add resistance, which is treated as timevarying disturbance and compensated in surge speed control.
Compared to the control results of the SMPC method, the surge speed has no significant fluctuations in path following under the control of the MBD method, which demonstrates the superiority of the MBD method. e pathfollowing results under the unknown time-varying disturbance demonstrate the effectiveness and robustness of the MBD method.

Data Availability
e data used to support the findings of this study are included within the article.