1 Introduction

The solution of the nonlinear optimal control problem for 3-DOF unmanned surface vessels (USVs) remains always a technical challenge [1,2,3,4]. The dynamic model of 3-DOF USVs is a highly nonlinear and multivariable one [5,6,7]. The vessel’s model can be fully actuated (when receiving as control inputs the engine’s forward propulsion, the rudder’s torque and the thrusters’ lateral propulsion. On the other side this dynamic model can be underactuated when thrusters’ lateral propulsion is missing [8,9,10]. Solution of the control problem for USVs enables autonomous navigation of the individual vessels or coordination of multiple vessels’ motion [11,12,13]. Several nonlinear control approaches, including robust and adaptive control schemes, have been developed. Precise tracking of reference setpoints and global stability properties of the USVs control loop are the design objectives in all related nonlinear control methods. Results on autonomous navigation of USVs with the use of adaptive control methods can be found in [14,15,16,17]. Results on the use of sliding-mode and robust control methods for solving the autonomous navigation problem of USVs are given in [18,19,20,21]. The use of observers and disturbance estimators in control loops of USVs can be found in [12,13,14,15,16,17,18,19,20,21,22,23,24]. Additional results on nonlinear control of 3-DOF USVs under disturbances can be found in [25,26,27,28]. Besides, recent developments on USV control under model uncertainty and perturbations are given in [29,30,31].

In the present article, a new nonlinear optimal control approach is developed for the dynamic model of 3-DOF unmanned surface vessels (USVs) [1]. This model is in the typical form of a robotic system, comprising an inertia matrix, a Coriolis and a centrifugal forces matrix, while receiving inputs from propulsion forces and rudder torques. It is shown that this model can be written in the affine-in-the-input nonlinear state-space form. Besides, it is proven that in the case of full actuation it satisfies differential flatness properties. Next, the dynamic model of the 3-DOF USV undergoes approximate linearization with the use of first-order Taylor series expansion and through the computation of the associated Jacobian matrices [32,33,34,35]. Linearization takes place around a temporary operating point which is recomputed at each time-step of the control algorithm. This operating point is redefined at each sampling instance by the present value of the USV’s state vector and by the last sampled value of the control inputs vector. The modelling error which is due to truncation of higher-order terms in the Taylor series expansion is viewed as a perturbation that is asymptotically compensated by the robustness of the control algorithm.

For the approximately linearized dynamic model of the 3-DOF USV a stabilizing H-infinity feedback controller is designed [1]. This controller achieves the solution of the optimal control problem for the dynamics of the 3-DOF USV under model uncertainties and external disturbances. It can be stated that the H-infnity-controller represents a min-max differential game taking place between (i) the control inputs of the USV which try to minimize a quadratic cost function of the state vector’s tracking error and (ii) the model uncertainty and external disturbance terms which try to maximize this cost function. To select the stabilizing feedback gains of the H-infinity controller an algebraic Riccati equation is repetitively solved at each time-step of the control method [36,37,38]. The global stability properties of the control scheme are proven through Lyapunov analysis. First, it is demonstrated that the control loop of the USV satisfies the H-infinity tracking performance criterion [1, 39]. This signifies elevated robustness under model imprecision and external perturbations. Additionally, to implement state estimation-based control without the need to measure the entire state vector of the USV, the H-infinity Kalman Filter is used as a robust state estimator [1, 40].

The proposed nonlinear optimal control method retains the known advantages of linear optimal control, that is fast and accurate tracking of reference setpoints under moderate variations of the control inputs. Comparing to past attempts to solve the nonlinear optimal (H-infinity) control problem, the following advancements can be noted: (i) in past results, the linearization of the system’s dynamics is performed at points of the desirable trajectories (setpoints), whereas in the article’s control method linearization is performed around a time-varying operating point which is redefined at each sampling instance by the present value of the system’s state vector and by the last sampled value of the control inputs vector, (ii) in past results the system is considered to be exclusively in the affine-in-the-input state-space form with a control inputs gain matrix which is time-invariant. On the contrary, in the present article’s approach there is no constraint to find the system in the affine-in-the-input state space form, while even in such a state-space description the control inputs gain matrix can be time-varying, (iii) in past approaches a different Lyapunov analysis procedure has been used to prove stability properties. In the present article, a novel Lyapunov function has been introduced to prove the global stability properties of the nonlinear optimal control scheme.

The structure of the article is defined as follows: in Sect. 2 the nonlinear dynamic model of the 3-DOF USV is formulated. The differential flatness properties of the model are proven. In Sect. 3 the dynamic model of the USV undergoes approximate linearization with the use of first-order Taylor series expansion and through the computation of the associated Jacobian matrices. In Sect. 4 the global stability properties of the control scheme are proven through Lyapunov analysis. In Sect. 5 the fine performance of the control method is further confirmed through simulation experiments. Finally, in Sect. 6 concluding remarks are stated.

2 Dynamic model of the unmanned surface vessel

2.1 Dynamics of the USV

The motion of the Unmanned Surface Vessle (USV) is described in both a body-fixed reference frame and in an inertial (earth-fixed) reference frame (Fig 1). In the body-fixed reference frame the joint kinematic and dynamic model of the USV comprises the following two-equations in matrix form [1, 3, 36]:

$$\begin{aligned}{} & {} \dot{\eta }=R(\psi )v{\Rightarrow }\dot{v}=R^{-1}(\psi ){\dot{\eta }}\nonumber \\{} & {} \quad M{\dot{v}}+Dv=\tau \end{aligned}$$
(1)

where \(\eta (t)=[x(t) \ y(t) \ \psi (t)]^T \ {\in }R^{3{\times }1}\) are the cartesian coordinates and the orientation angle of the vessel in the inertial reference frame OXYZ. The associate velocities vector in the inertial reference frame is \(\dot{\eta }(t)=[\dot{x}(t) \ \dot{y}(t) \ \dot{\psi }(t)]^T \ {\in }R^{3{\times }1}\). The velocities vector in the body-fixed reference frame \({\Sigma }{V_1}{V_2}{V_3}\) is denoted as \(v(t)=[v_1 \ v_2 \ v_3]^T\) where \(v_!\) is the surge velocity, \(v_2\) is the sway velocity and \(v_3\) is the yaw turn velocity of the vessel.

Fig. 1
figure 1

Monitoring of the motion of the USV in an inertial reference frame OXYZ as well as in a body-fixed reference \(\Sigma {V_1}{V_2}{V_3}\)

Full size image

In the above model, matrices M, D and R are given by [1, 3, 36]

$$\begin{aligned}{} & {} M=\begin{pmatrix} m_{11} &{} 0 &{} 0 \\ 0 &{} m_{22} &{} m_{33} \\ 0 &{} m_{23} &{} m_{33} \end{pmatrix} D=\begin{pmatrix} d_{11} &{} 0 &{} 0 \\ 0 &{} d_{22} &{} d_{23} \\ 0 &{} d_{22} &{} d_{33} \end{pmatrix} \nonumber \\{} & {} \quad R=\begin{pmatrix} cos(\psi ) &{} -sin(\psi ) &{} 0 \\ sin(\psi ) &{} cos(\psi ) &{} 0 \\ 0 &{} 0 &{} 1 \end{pmatrix} \end{aligned}$$
(2)

The dynamic model of the USV is expressed next exclusively in the inertial reference frame. Using the previous relations it holds that [1, 3, 36]

$$\begin{aligned} \tilde{J}\ddot{\eta }+\tilde{C}(\eta ,\dot{\eta })\dot{\eta }+\tilde{F}\dot{\eta }=\tau ^{*} \end{aligned}$$
(3)

where \(\tilde{J}=RMR^T \ {\in }R^{3{\times }3}\), \(\tilde{C}(\eta ,\dot{\eta })=RM{\dot{R}^T} \ {\in }R^{3{\times }3}\), \(\tilde{F}=RD{R^T} \ {\in }R^{3{\times }3}\) and \(\tau ^{*}=R{\tau } \in \ {R^{3{\times }1}}\). It holds that

$$\begin{aligned}{} & {} \tilde{J}(\eta )=\begin{pmatrix} {m_{11}}cos^2(\psi )+{m_{22}}sin^2(\psi ) &{} (m_{11}-m_{22})sin(\psi )cos(\psi ) &{} -m_{23}sin(\psi ) \\ (m_{11}-m_{22})sin(\psi )cos(\psi ) &{} {m_{11}}{sin^2}(\psi )+{m_{22}}cos^2(\psi ) &{} m_{23}cos(\psi ) \\ -{m_{23}}sin(\psi ) &{} {m_{23}}cos(\psi ) &{} m_{33} \end{pmatrix} \end{aligned}$$
(4)
$$\begin{aligned}{} & {} \tilde{C}(\eta ,\dot{\eta })=\begin{pmatrix} \dot{\psi }(m_{22}-m_{11})sin(\psi )cos(\psi ) &{} \dot{\psi }(m_{11}cos^2(\psi )+{m_{22}}sin^2(\psi )) &{} 0 \\ -\dot{\psi }({m_{11}}sin^2(\psi )+{m_{22}}cos^2(\psi )) &{} {\dot{\psi }}(m_{11}-m_{22})sin(\psi )cos(\psi ) &{} 0 \\ -\dot{\psi }{m_{23}}cos(\psi ) &{} -\dot{\psi }{m_{23}}sin(\psi ) &{} 0 \end{pmatrix} \end{aligned}$$
(5)
$$\begin{aligned}{} & {} \tilde{F}(\eta )=\begin{pmatrix} {d_{11}}cos^2(\psi )+{d_{22}}sin^2(\psi ) &{} ({d_{11}}-d_{22})sin(\psi )cos(\psi ) &{} -d_{23}sin(\psi ) \\ ({d_{11}}-d_{22})sin(\psi )cos(\psi ) &{} {d_{11}}sim^2(\psi )+{d_{22}}cos^2(\psi ) &{} {d_{23}}cos(\psi ) \\ -{d_{32}}sin(\psi ) &{} {d_{22}}cos(\psi ) &{} {d_{23}} \end{pmatrix} \end{aligned}$$
(6)

Using Eq. (3) the dynamic model of the USV can be written as [1, 3, 36]

$$\begin{aligned}{} & {} {\tilde{J}(\eta )}\ddot{\eta }+\tilde{C}(\eta ,\dot{\eta })\dot{\eta }+\tilde{F}(\eta )\dot{\eta } =\tau ^{*}{\Rightarrow }\nonumber \\{} & {} \quad \ddot{\eta }=-\tilde{J}^{-1}(\eta )\tilde{C}(\eta ,\dot{\eta })\dot{\eta } \nonumber \\{} & {} \quad -\tilde{J}^{-1}(\eta )\tilde{F}(\eta )\dot{\eta }+\tilde{J}^{-1}(\eta )\tau ^{*}{\Rightarrow }\nonumber \\{} & {} \quad \ddot{\eta }=-\tilde{J}^{-1}(\eta )[\tilde{C}(\eta ,\dot{\eta })+\tilde{F}(\eta )]\dot{\eta }+\tilde{J}^{-1}\tau ^{*} \end{aligned}$$
(7)

The inverse matrix \(\tilde{J}^{-1}(\eta )\) is computed first. It holds that

$$\begin{aligned} \tilde{J}(\eta )=\begin{pmatrix} \tilde{J}_{11} &{} \tilde{J}_{12} &{} \tilde{J}_{13} \\ \tilde{J}_{21} &{} \tilde{J}_{22} &{} \tilde{J}_{23} \\ \tilde{J}_{31} &{} \tilde{J}_{32} &{} \tilde{J}_{33} \end{pmatrix} \end{aligned}$$
(8)

with \(\tilde{J}_{11}={m_{11}}cos^2(\psi )+{m_{22}}sin^2(\psi )\), \(\tilde{J}_{12}=(m_{11}-m_{22})sin(\psi )cos(\psi )\), \(\tilde{J}_{13}=-m_{23}sin(\psi )\), \(\tilde{J}_{21}=(m_{11}-m_{22})sin(\psi )cos(\psi )\), \(\tilde{J}_{22}={m_{11}}{sin^2}(\psi )+{m_{22}}cos^2(\psi )\), \(\tilde{J}_{23}=m_{23}cos(\psi )\), \(\tilde{J}_{31}=-{m_{23}}sin(\psi )\), \(\tilde{J}_{32}={m_{23}}cos(\psi )\) and \(J_{33}=m_{33}\).

The determinant of matrix \(\tilde{J}(\eta ,\dot{\eta })\) is

$$\begin{aligned}{} & {} detJ=\tilde{J}_{11}(\tilde{J}_{22}\tilde{J}_{33}-\tilde{J}_{32}\tilde{J}_{23})-\nonumber \\{} & {} \quad -\tilde{J}_{12}(\tilde{J}_{21}\tilde{J}_{33}-\tilde{J}_{31}\tilde{J}_{23})+\nonumber \\{} & {} \quad +\tilde{J}_{13}(\tilde{J}_{21}\tilde{J}_{32}-\tilde{J}_{31}\tilde{J}_{22}) \end{aligned}$$
(9)

The sub-determinants of matrix \(\tilde{J}(\eta ,\dot{\eta })\) are defined as: \(J_{11}={\tilde{J}_{22}}{\tilde{J}_{33}}- {\tilde{J}_{32}}{\tilde{J}_{23}}\), \(J_{12}={\tilde{J}_{21}}{\tilde{J}_{33}}- {\tilde{J}_{31}}{\tilde{J}_{23}}\), \(J_{13}={\tilde{J}_{21}}{\tilde{J}_{32}}- {\tilde{J}_{31}}{\tilde{J}_{22}}\), \(J_{21}={\tilde{J}_{12}}{\tilde{J}_{33}}- {\tilde{J}_{32}}{\tilde{J}_{13}}\), \(J_{22}={\tilde{J}_{11}}{\tilde{J}_{33}}- {\tilde{J}_{31}}{\tilde{J}_{33}}\), \(J_{23}={\tilde{J}_{11}}{\tilde{J}_{22}}- {\tilde{J}_{31}}{\tilde{J}_{12}}\), \(J_{31}={\tilde{J}_{12}}{\tilde{J}_{23}}- {\tilde{J}_{22}}{\tilde{J}_{13}}\), \(J_{32}={\tilde{J}_{11}}{\tilde{J}_{23}}- {\tilde{J}_{21}}{\tilde{J}_{13}}\), \(J_{33}={\tilde{J}_{11}}{\tilde{J}_{22}}- {\tilde{J}_{21}}{\tilde{J}_{12}}\).

Thus, the inverse of matrix \(\tilde{J}(\eta )\) is

$$\begin{aligned}{} & {} \tilde{J}^{-1}(\eta )={1 \over {det\tilde{J}}} \begin{pmatrix} J_{11} &{} -J_{21} &{} J_{31} \\ -J_{12} &{} J_{22} &{} -J_{32} \\ J_{13} &{} -J_{23} &{} J_{33} \\ \end{pmatrix} \end{aligned}$$
(10)

Moreover the vector of the Coriolis and centrifugal forces \(\tilde{C}(\eta ,\dot{\eta })\dot{\eta }\) is written in the following form

$$\begin{aligned} C(\eta ,\dot{\eta })=\begin{pmatrix} c_1 \\ c_2 \\ c_3 \end{pmatrix}= \begin{pmatrix} {\tilde{C}_{11}}{\dot{\eta }_1}+{\tilde{C}_{12}}{\dot{\eta }_2}+{\tilde{C}_{13}}{\dot{\eta }_3} \\ {\tilde{C}_{21}}{\dot{\eta }_1}+{\tilde{C}_{22}}{\dot{\eta }_2}+{\tilde{C}_{23}}{\dot{\eta }_3} \\ {\tilde{C}_{31}}{\dot{\eta }_1}+{\tilde{C}_{32}}{\dot{\eta }_2}+{\tilde{C}_{33}}{\dot{\eta }_3} \end{pmatrix} \end{aligned}$$
(11)

where \(\tilde{C}_{11}=\dot{\psi }(m_{22}-m_{11})sin(\psi )cos(\psi )\), \(\tilde{C}_{12}=\dot{\psi }(m_{11}cos^2(\psi )+{m_{22}}sin^2(\psi ))\), \(\tilde{C}_{13}=0\), \(\tilde{C}_{21}=-\dot{\psi }({m_{11}}sin^2(\psi )+{m_{22}}cos^2(\psi ))\), \(\tilde{C}_{22}={\dot{\psi }}(m_{11}-m_{22})sin(\psi )cos(\psi )\), \(\tilde{C}_{23}=0\), \(\tilde{C}_{31}=-\dot{\psi }{m_{23}}cos(\psi )\), \(\tilde{C}_{32}=-\dot{\psi }{m_{23}}sin(\psi )\), \(\tilde{C}_{33}=0\).

Additionally, the damping forces vector \(\tilde{F}(\eta )\dot{\eta }\) can be written in the form:

$$\begin{aligned} Q(\eta ,\dot{\eta })=\begin{pmatrix} q_1 \\ q_2 \\ q_3 \end{pmatrix}= \begin{pmatrix} \tilde{F}_{11}\dot{\eta }_1+ \tilde{F}_{12}\dot{\eta }_2+\tilde{F}_{13}\dot{\eta }_3 \\ \tilde{F}_{21}\dot{\eta }_1+ \tilde{F}_{22}\dot{\eta }_2+\tilde{F}_{23}\dot{\eta }_3 \\ \tilde{F}_{31}\dot{\eta }_1+ \tilde{F}_{32}\dot{\eta }_2+\tilde{F}_{33}\dot{\eta }_3 \end{pmatrix} \end{aligned}$$
(12)

where \(\tilde{F}_{11}={d_{11}}cos^2(\psi )+{d_{22}}sin^2(\psi )\), \(\tilde{F}_{12}=({d_{11}}-d_{22})sin(\psi )cos(\psi )\), \(\tilde{F}_{13}=-d_{23}sin(\psi )\), \(\tilde{F}_{21}=({d_{11}}-d_{22})sin(\psi )cos(\psi )\), \(\tilde{F}_{22}={d_{11}}sim^2(\psi )+{d_{22}}cos^2(\psi )\), \(\tilde{F}_{23}={d_{23}}cos(\psi )\), \(\tilde{F}_{31}=-{d_{32}}sin(\psi )\), \(\tilde{F}_{32}={d_{22}}cos(\psi )\), \(\tilde{F}_{33}={d_{23}}\).

Consequently, the dynamic model of the autonomous surface vessel is written as

$$\begin{aligned} \ddot{\eta }=-{\tilde{J}^{-1}}(\eta )C(\eta ,\dot{\eta })-{\tilde{J}^{-1}}(\eta )Q(\eta ,\dot{\eta }) +{\tilde{J}^{-1}}(\eta )\tau ^{*} \end{aligned}$$
(13)

or equivalently

$$\begin{aligned}{} & {} \begin{pmatrix} \ddot{\eta }_1 \\ \ddot{\eta }_2 \\ \ddot{\eta }_3 \end{pmatrix}= -{1 \over {det\tilde{J}}} \begin{pmatrix} J_{11} &{} -J_{21} &{} J_{31} \\ -J_{12} &{} J_{22} &{} -J_{32} \\ J_{13} &{} -J_{23} &{} J_{33} \end{pmatrix} \begin{pmatrix} c_1 \\ c_2 \\ c_3 \end{pmatrix} \nonumber \\{} & {} \quad -{1 \over {det\tilde{J}}} \begin{pmatrix} J_{11} &{} -J_{21} &{} J_{31} \\ -J_{12} &{} J_{22} &{} -J_{32} \\ J_{13} &{} -J_{23} &{} J_{33} \end{pmatrix} \begin{pmatrix} q_1 \\ q_2 \\ q_3 \end{pmatrix}+\nonumber \\{} & {} \quad +{1 \over {det\tilde{J}}} \begin{pmatrix} J_{11} &{} -J_{21} &{} J_{31} \\ -J_{12} &{} J_{22} &{} -J_{32} \\ J_{13} &{} -J_{23} &{} J_{33} \end{pmatrix} \begin{pmatrix} \tau _1 \\ \tau _2 \\ \tau _3 \end{pmatrix} \end{aligned}$$
(14)

or similarly

$$\begin{aligned}{} & {} \begin{pmatrix} \ddot{\eta }_1 \\ \ddot{\eta }_2 \\ \ddot{\eta }_3 \end{pmatrix}= -{1 \over {det\tilde{J}}} \begin{pmatrix} J_{11} &{} -J_{21} &{} J_{31} \\ -J_{12} &{} J_{22} &{} -J_{32} \\ J_{13} &{} -J_{23} &{} J_{33} \end{pmatrix} \begin{pmatrix} c_1+q_1 \\ c_2+q_2 \\ c_3+q_3 \end{pmatrix}\nonumber \\{} & {} \quad +{1 \over {det\tilde{J}}} \begin{pmatrix} J_{11} &{} -J_{21} &{} J_{31} \\ -J_{12} &{} J_{22} &{} -J_{32} \\ J_{13} &{} -J_{23} &{} J_{33} \end{pmatrix} \begin{pmatrix} \tau _1 \\ \tau _2 \\ \tau _3 \end{pmatrix} \end{aligned}$$
(15)

Consequently, the dynamic model of the unmanned surface vessel can be written as

$$\begin{aligned}{} & {} \begin{pmatrix} \ddot{\eta }_1 \\ \\ \ddot{\eta }_2 \\ \\ \ddot{\eta }_3 \end{pmatrix}= \begin{pmatrix} {{-J_{11}(c_1+q_1)+J_{21}(c_2+q_2)-J_{31}(c_3+q_3)} \over {detJ}} \\ \\ {{J_{12}(c_1+q_1)-J_{22}(c_2+q_2)+J_{32}(c_3+q_3)} \over {detJ}} \\ \\ {{-J_{13}(c_1+q_1)+J_{23}(c_2+q_2)-J_{33}(c_3+q_3)} \over {detJ}} \end{pmatrix}+ \begin{pmatrix} {J_{11} \over detJ} \\ \\ -{J_{12} \over detJ} \\ \\ {J_{13} \over detJ} \\ \end{pmatrix}\nonumber \\{} & {} \quad \tau _1+ \begin{pmatrix} -{J_{21} \over detJ} \\ \\ {J_{22} \over detJ} \\ \\ -{J_{23} \over detJ} \\ \end{pmatrix}\tau _2+ \begin{pmatrix} {J_{31} \over detJ} \\ \\ -{J_{32} \over detJ} \\ \\ {J_{33} \over detJ} \\ \end{pmatrix}\tau _3 \end{aligned}$$
(16)

Next, the following state vector is defined for the USV \(x=[x_1,x_2,x_3,x_4,x_5,x_6,x_7,x_8]^T\), or \(x=[\eta _1,\dot{\eta }_1,\eta _2,\dot{\eta }_2,\eta _3,\dot{\eta }_3]^T\), that is \(x=[x,\dot{x},y,\dot{y},\psi ,\dot{\psi }]^T\). Besides, the control inputs vector is defined as \(u=[u_1,u_2,u_3]^T\) or \(u=[\tau _1,\tau _2,\tau _3]^T\). Then, the dynamic model of the USV is written as

$$\begin{aligned}{} & {} \begin{pmatrix} \dot{x}_1 \\ \dot{x}_2 \\ \dot{x}_3 \\ \dot{x}_4 \\ \dot{x}_5 \\ \dot{x}_6 \\ \end{pmatrix}= \begin{pmatrix} x_2 \\ {{-J_{11}(c_1+q_1)+J_{21}(c_2+q_2)-J_{31}(c_3+q_3)} \over {detJ}} \\ x_4 \\ {{J_{12}(c_1+q_1)-J_{22}(c_2+q_2)+J_{32}(c_3+q_3)} \over {detJ}} \\ x_6 \\ {{-J_{13}(c_1+q_1)+J_{23}(c_2+q_2)-J_{33}(c_3+q_3)} \over {detJ}} \end{pmatrix}+ \begin{pmatrix} 0\\ {J_{11} \over detJ} \\ 0 \\ -{J_{12} \over detJ} \\ 0 \\ {J_{13} \over detJ} \\ \end{pmatrix}\nonumber \\{} & {} \quad u_1+ \begin{pmatrix} 0\\ -{J_{21} \over detJ} \\ 0 \\ {J_{22} \over detJ} \\ 0 \\ -{J_{23} \over detJ} \\ \end{pmatrix}u_2+ \begin{pmatrix} 0\\ {J_{31} \over detJ} \\ 0 \\ -{J_{32} \over detJ} \\ 0 \\ {J_{33} \over detJ} \\ \end{pmatrix}u_3 \end{aligned}$$
(17)

Thus, the dynamic model of the USV is written in the following nonlinear affine-in-the-input state-space form:

$$\begin{aligned} \dot{x}=f(x)+g_1(x)u_1+g_2(x)u_2+g_3(x)u_3 \end{aligned}$$
(18)

with \(x{\in }R^{6{\times }1}\), \(f(x){\in }R^{6{\times }1}\), \(g_1(x){\in }R^{6{\times }1}\), \(g_2(x){\in }R^{6{\times }1}\), \(g_3(x){\in }R^{6{\times }1}\) and \(u{\in }R^{3{\times }1}\). Next, the elements of the state-space description of the USV are written as functions of the state vector elements \(x_1\) to \(x_6\).

Thus \(J_{11}={m_{11}}cos^2(x_5)+{m_{22}}sin^2(x_5)\), \(J_{12}=(m_{11}-m_{22})sin(x_5)cos(x_5)\), \(J_{13}=-m_{23}sin(x_5)\), \(J_{21}=(m_{11}-m_{22})sin(x_5)cos(x_5)\), \(J_{22}={m_{11}}{sin^2}(x_5)+{m_{22}}cos^2(x_5)\), \(J_{23}=m_{23}cos(x_5)\), \(J_{31}=-{m_{23}}sin(x_5)\), \(J_{32}={m_{23}}cos(x_5)\) and \(J_{33}=m_{33}\).

and \(\tilde{C}_{11}=x_6(m_{22}-m_{11})sin(x_5)cos(x_5)\), \(\tilde{C}_{12}=x_6(m_{11}cos^2(x_5)+{m_{22}}sin^2(x_5))\), \(\tilde{C}_{13}=0\), \(\tilde{C}_{21}=-x_6({m_{11}}sin^2(x_5)+{m_{22}}cos^2(x_5))\), \(\tilde{C}_{22}=x_6(m_{11}-m_{22})sin(x_5)cos(x_5)\), \(\tilde{C}_{23}=0\), \(\tilde{C}_{31}=-x_6{m_{23}}cos(x_5)\), \(\tilde{C}_{32}=-x_6{m_{23}}sin(x_5)\), \(\tilde{C}_{33}=0\).

and \(\tilde{F}_{11}={d_{11}}cos^2(x_5)+{d_{22}}sin^2(x_5)\), \(\tilde{F}_{12}=({d_{11}}-d_{22})sin(x_5)cos(x_5)\), \(\tilde{F}_{13}=-d_{23}sin(x_5)\), \(\tilde{F}_{21}=({d_{11}}-d_{22})sin(x_5)cos(x_5)\), \(\tilde{F}_{22}={d_{11}}sim^2(x_5)+{d_{22}}cos^2(x_5)\), \(\tilde{F}_{23}={d_{23}}cos(x_5)\), \(\tilde{F}_{31}=-{d_{32}}sin(x_5)\), \(\tilde{F}_{32}={d_{22}}cos(x_5)\), \(\tilde{F}_{33}={d_{23}}\).

2.2 Differential flatness properties of the USV dynamic model

It will be proven that the dynamic model of the 3-DOF USV is differentially flat with flat outputs vector \(y=[x,y,\psi ]^T\) of \(y=[x_1,x_3,x_5]^T\). From the first, third and fifth rows of the state-space model of the USV one obtains

$$\begin{aligned} x_2=\dot{x}_1 \ \ x_4=\dot{x}_3 \ \ x_6=\dot{x}_5 \end{aligned}$$
(19)

This signifies that all state variables of the USV can be written as differential functions of the system’s flat outputs. Besides, the state-space model of the USV can be written as

$$\begin{aligned}{} & {} \begin{pmatrix} \ddot{x}_1 \\ \\ \ddot{x}_3 \\ \\ \ddot{x}_5 \end{pmatrix}= \begin{pmatrix} {{-J_{11}(c_1+q_1)+J_{21}(c_2+q_2)-J_{31}(c_3+q_3)} \over {detJ}} \\ \\ {{J_{12}(c_1+q_1)-J_{22}(c_2+q_2)+J_{32}(c_3+q_3)} \over {detJ}} \\ \\ {{J_{13}(c_1+q_1)-J_{23}(c_2+q_2)+J_{33}(c_3+q_3)} \over {detJ}} \end{pmatrix}\nonumber \\{} & {} \quad + \begin{pmatrix} {J_{11} \over detJ} &{} -{J_{21} \over detJ} &{} {J_{31} \over detJ} \\ \\ -{J_{12} \over detJ} &{} {J_{22} \over detJ} &{} -{J_{32} \over detJ} \\ \\ {J_{13} \over detJ} &{} -{J_{23} \over detJ} &{} {J_{33} \over detJ} \\ \end{pmatrix} \begin{pmatrix} u_1 \\ \\ u_2 \\ \\ u_3 \end{pmatrix} \end{aligned}$$
(20)

or equivalently

$$\begin{aligned}{} & {} \begin{pmatrix} u_1 \\ \\ u_2 \\ \\ u_3 \end{pmatrix}= \begin{pmatrix} {J_{11} \over detJ} &{} -{J_{21} \over detJ} &{} {J_{31} \over detJ} \\ \\ -{J_{12} \over detJ} &{} {J_{22} \over detJ} &{} -{J_{32} \over detJ} \\ \\ {J_{13} \over detJ} &{} -{J_{23} \over detJ} &{} {J_{33} \over detJ} \\ \end{pmatrix}^{-1}\nonumber \\{} & {} \quad \begin{bmatrix} \begin{pmatrix} \ddot{x}_1 \\ \\ \ddot{x}_3 \\ \\ \ddot{x}_5 \end{pmatrix}- \begin{pmatrix} {{-J_{11}(c_1+q_1)+J_{21}(c_2+q_2)-J_{31}(c_3+q_3)} \over {detJ}} \\ \\ {{J_{12}(c_1+q_1)-J_{22}(c_2+q_2)+J_{32}(c_3+q_3)} \over {detJ}} \\ \\ {{-J_{13}(c_1+q_1)+J_{23}(c_2+q_2)-J_{33}(c_3+q_3)} \over {detJ}} \end{pmatrix} \end{bmatrix} \end{aligned}$$
(21)

Consequently, the control inputs of the USV are also differential functions of the flat outputs and the entire system is differentially flat. The differential flatness property of the USV can be used for defining setpoints for the system’s state variables.

3 Approximate linearization of the USV state-space model

3.1 Approximately linearized dynamics of the USV

The dynamic model of the USV undergoes approximate linearization around the temporary operating point \((x^{*},u^{*})\), where \(x^{*}\) is the present value of the USV’s state vector and \(u^{*}\) is the last sampled value of the control inputs vector. The linearization relies on first-order Taylor series expansion and on the computation of the associated Jacobian matrices at each sampling instance. The initial nonlinear model of the USV

$$\begin{aligned}{} & {} \dot{x}=f(x,u) \ \ \text {or}\nonumber \\{} & {} \quad \dot{x}=f(x)+{g_1}(x)u_1+g_2(x)u_2+g_3(x)u_3 \end{aligned}$$
(22)

is substituted by its equivalent linearized description

$$\begin{aligned} \dot{x}=Ax+Bu+\tilde{d} \end{aligned}$$
(23)

where \(\tilde{d}\) is the cumulative disturbances vector. This may comprise (i) the modelling error due to truncation of higher-order terms in the Taylor series expansion, (ii) exogenous perturbations, (iii) sensor measurement noise of any distribution.

Matrices A and B are Jacobians of the linearization process and are given by

$$\begin{aligned}{} & {} \quad A={\nabla _x}[f(x)+{g_1}(x)u_1+g_2(x)u_2+g_3(x)u_3]\mid _{(x^{*},u^{*})}{\Rightarrow }\nonumber \\{} & {} \quad A={\nabla _x}f(x)\mid _{(x^{*},u^{*})}+{\nabla _x}{g_1}(x){u_1}\mid _{(x^{*},u^{*})} +{\nabla _x}{g_2}(x){u_2}\mid _{(x^{*},u^{*})}\nonumber \\{} & {} \quad +{\nabla _x}{g_3}(x){u_3}\mid _{(x^{*},u^{*})} \end{aligned}$$
(24)
$$\begin{aligned}{} & {} B={\nabla _u}[f(x)+{g_1}(x)u_1+g_2(x)u_2+g_3(x)u_3]\mid _{(x^{*},u^{*})}{\Rightarrow }\nonumber \\{} & {} \quad B=[g_1(x) \ g_2(x) \ g_3(x)]\mid _{(x^{*},u^{*})}{\Rightarrow }B=g(x)\mid _{(x^{*},u^{*})} \end{aligned}$$
(25)

Computation of the Jacobian matrix \({\nabla _x}f(x)\mid _{(x^{*},u^{*})}\).

First row of the Jacobian matrix \({\nabla _x}f(x)\mid _{(x^{*},u^{*})}\): \({{{\partial }f_1} \over {{\partial }x_1}}=0\), \({{{\partial }f_1} \over {{\partial }x_2}}=1\), \({{{\partial }f_1} \over {{\partial }x_3}}=0\), \({{{\partial }f_1} \over {{\partial }x_4}}=0\), \({{{\partial }f_1} \over {{\partial }x_5}}=0\), and \({{{\partial }f_1} \over {{\partial }x_6}}=0\).

Second row of the Jacobian matrix \({\nabla _x}f(x)\mid _{(x^{*},u^{*})}\): for \(i=1,2,\cdots ,6\)

$$\begin{aligned}{} & {} {{{\partial }f_2} \over {{\partial }x_i}}=-{{[{{{\partial }J_{11}} \over {{\partial }x_i}}(c_1+q_1)+J_{11}({{{\partial }c_1} \over {{\partial }x_i}}+{{{\partial }q_1} \over {{\partial }x_i}})]detJ-[J_{11}(c_1+q_1)]{{{\partial }detJ} \over {{\partial }x_i}}} \over {detJ^2}}+\nonumber \\{} & {} \quad +{{[{{{\partial }J_{21}} \over {{\partial }x_i}}(c_2+q_2)+J_{21}({{{\partial }c_2} \over {{\partial }x_i}}+{{{\partial }q_2} \over {{\partial }x_i}})]detJ-[J_{21}(c_2+q_2)]{{{\partial }detJ} \over {{\partial }x_i}}} \over {detJ^2}}-\nonumber \\{} & {} \quad -{{[{{{\partial }J_{31}} \over {{\partial }x_i}}(c_3+q_3)+J_{31}({{{\partial }c_3} \over {{\partial }x_i}}+{{{\partial }q_3} \over {{\partial }x_i}})]detJ-[J_{31}(c_3+q_3)]{{{\partial }detJ} \over {{\partial }x_i}}} \over {detJ^2}} \end{aligned}$$
(26)

Third row of the Jacobian matrix \({\nabla _x}f(x)\mid _{(x^{*},u^{*})}\): \({{{\partial }f_3} \over {{\partial }x_1}}=0\), \({{{\partial }f_3} \over {{\partial }x_2}}=0\), \({{{\partial }f_3} \over {{\partial }x_3}}=0\), \({{{\partial }f_3} \over {{\partial }x_4}}=1\), \({{{\partial }f_3} \over {{\partial }x_5}}=0\), and \({{{\partial }f_3} \over {{\partial }x_6}}=0\).

Fourth row of the Jacobian matrix \({\nabla _x}f(x)\mid _{(x^{*},u^{*})}\): for \(i=1,2,\cdots ,6\)

$$\begin{aligned}{} & {} {{{\partial }f_4} \over {{\partial }x_i}}={{[{{{\partial }J_{12}} \over {{\partial }x_i}}(c_1+q_1)+J_{12}({{{\partial }c_1} \over {{\partial }x_i}}+{{{\partial }q_1} \over {{\partial }x_i}})]detJ-[J_{12}(c_1+q_1)]{{{\partial }detJ} \over {{\partial }x_i}}} \over {detJ^2}}-\nonumber \\{} & {} \quad -{{[{{{\partial }J_{22}} \over {{\partial }x_i}}(c_2+q_2)+J_{22}({{{\partial }c_2} \over {{\partial }x_i}}+{{{\partial }q_2} \over {{\partial }x_i}})]detJ-[J_{22}(c_2+q_2)]{{{\partial }detJ} \over {{\partial }x_i}}} \over {detJ^2}}+\nonumber \\{} & {} \quad +{{[{{{\partial }J_{32}} \over {{\partial }x_i}}(c_3+q_3)+J_{32}({{{\partial }c_3} \over {{\partial }x_i}}+{{{\partial }q_3} \over {{\partial }x_i}})]detJ-[J_{32}(c_3+q_3)]{{{\partial }detJ} \over {{\partial }x_i}}} \over {detJ^2}} \end{aligned}$$
(27)

Fifth row of the Jacobian matrix \({\nabla _x}f(x)\mid _{(x^{*},u^{*})}\): \({{{\partial }f_5} \over {{\partial }x_1}}=0\), \({{{\partial }f_5} \over {{\partial }x_2}}=0\), \({{{\partial }f_5} \over {{\partial }x_3}}=0\), \({{{\partial }f_5} \over {{\partial }x_4}}=0\), \({{{\partial }f_5} \over {{\partial }x_5}}=0\), and \({{{\partial }f_5} \over {{\partial }x_6}}=1\).

Sixth row of the Jacobian matrix \({\nabla _x}f(x)\mid _{(x^{*},u^{*})}\): for \(i=1,2,\cdots ,6\)

$$\begin{aligned}{} & {} {{{\partial }f_6} \over {{\partial }x_i}}=-{{[{{{\partial }J_{13}} \over {{\partial }x_i}}(c_1+q_1)+J_{12}({{{\partial }c_1} \over {{\partial }x_i}}+{{{\partial }q_1} \over {{\partial }x_i}})]detJ-[J_{13}(c_1+q_1)]{{{\partial }detJ} \over {{\partial }x_i}}} \over {detJ^2}}+\nonumber \\{} & {} \quad +{{[{{{\partial }J_{23}} \over {{\partial }x_i}}(c_2+q_2)+J_{23}({{{\partial }c_2} \over {{\partial }x_i}}+{{{\partial }q_2} \over {{\partial }x_i}})]detJ-[J_{23}(c_2+q_2)]{{{\partial }detJ} \over {{\partial }x_i}}} \over {detJ^2}}-\nonumber \\{} & {} \quad -{{[{{{\partial }J_{33}} \over {{\partial }x_i}}(c_3+q_3)+J_{33}({{{\partial }c_3} \over {{\partial }x_i}}+{{{\partial }q_3} \over {{\partial }x_i}})]detJ-[J_{33}(c_3+q_3)]{{{\partial }detJ} \over {{\partial }x_i}}} \over {detJ^2}} \end{aligned}$$
(28)

Computation of the Jacobian matrix \({\nabla _x}{g_1}(x)\mid _{(x^{*},u^{*})}\).

First row of the Jacobian matrix \({\nabla _x}{g_1}(x)\mid _{(x^{*},u^{*})}\): \({{{\partial }g_{11}} \over {{\partial }x_i}}=0\), for \(i=1,2,\cdots ,6\).

Second row of the Jacobian matrix \({\nabla _x}{g_1}(x)\mid _{(x^{*},u^{*})}\) for \(i=1,2,\cdots ,6\)

$$\begin{aligned} {{{\partial }g_{21}} \over {{\partial }x_i}}={{{{{\partial }J_{11}} \over {{\partial }x_i}}detJ-J_{11}{{{\partial }detJ} \over {{\partial }x_i}}} \over {detJ^2}} \end{aligned}$$
(29)

Third row of the Jacobian matrix \({\nabla _x}{g_1}(x)\mid _{(x^{*},u^{*})}\): \({{{\partial }g_{31}} \over {{\partial }x_i}}=0\), for \(i=1,2,\cdots ,6\).

Fourth row of the Jacobian matrix \({\nabla _x}{g_1}(x)\mid _{(x^{*},u^{*})}\) for \(i=1,2,\cdots ,6\)

$$\begin{aligned} {{{\partial }g_{41}} \over {{\partial }x_i}}=-{{{{{\partial }J_{12}} \over {{\partial }x_i}}detJ-J_{12}{{{\partial }detJ} \over {{\partial }x_i}}} \over {detJ^2}} \end{aligned}$$
(30)

Fifth row of the Jacobian matrix \({\nabla _x}{g_1}(x)\mid _{(x^{*},u^{*})}\): \({{{\partial }g_{51}} \over {{\partial }x_i}}=0\), for \(i=1,2,\cdots ,6\).

Sixth row of the Jacobian matrix \({\nabla _x}{g_1}(x)\mid _{(x^{*},u^{*})}\) for \(i=1,2,\cdots ,6\)

$$\begin{aligned} {{{\partial }g_{61}} \over {{\partial }x_i}}={{{{{\partial }J_{13}} \over {{\partial }x_i}}detJ-J_{13}{{{\partial }detJ} \over {{\partial }x_i}}} \over {detJ^2}} \end{aligned}$$
(31)

Computation of the Jacobian matrix \({\nabla _x}{g_2}(x)\mid _{(x^{*},u^{*})}\).

First row of the Jacobian matrix \({\nabla _x}{g_2}(x)\mid _{(x^{*},u^{*})}\): \({{{\partial }g_{12}} \over {{\partial }x_i}}=0\), for \(i=1,2,\cdots ,6\).

Second row of the Jacobian matrix \({\nabla _x}{g_2}(x)\mid _{(x^{*},u^{*})}\) for \(i=1,2,\cdots ,6\)

$$\begin{aligned} {{{\partial }g_{22}} \over {{\partial }x_i}}=-{{{{{\partial }J_{21}} \over {{\partial }x_i}}detJ-J_{21}{{{\partial }detJ} \over {{\partial }x_i}}} \over {detJ^2}} \end{aligned}$$
(32)

Third row of the Jacobian matrix \({\nabla _x}{g_2}(x)\mid _{(x^{*},u^{*})}\): \({{{\partial }g_{32}} \over {{\partial }x_i}}=0\), for \(i=1,2,\cdots ,6\).

Fourth row of the Jacobian matrix \({\nabla _x}{g_2}(x)\mid _{(x^{*},u^{*})}\) for \(i=1,2,\cdots ,6\)

$$\begin{aligned} {{{\partial }g_{42}} \over {{\partial }x_i}}={{{{{\partial }J_{22}} \over {{\partial }x_i}}detJ-J_{22}{{{\partial }detJ} \over {{\partial }x_i}}} \over {detJ^2}} \end{aligned}$$
(33)

Fifth row of the Jacobian matrix \({\nabla _x}{g_2}(x)\mid _{(x^{*},u^{*})}\): \({{{\partial }g_{52}} \over {{\partial }x_i}}=0\), for \(i=1,2,\cdots ,6\).

Sixth row of the Jacobian matrix \({\nabla _x}{g_2}(x)\mid _{(x^{*},u^{*})}\) for \(i=1,2,\cdots ,6\)

$$\begin{aligned} {{{\partial }g_{62}} \over {{\partial }x_i}}=-{{{{{\partial }J_{23}} \over {{\partial }x_i}}detJ-J_{23}{{{\partial }detJ} \over {{\partial }x_i}}} \over {detJ^2}} \end{aligned}$$
(34)

Computation of the Jacobian matrix \({\nabla _x}{g_3}(x)\mid _{(x^{*},u^{*})}\).

First row of the Jacobian matrix \({\nabla _x}{g_3}(x)\mid _{(x^{*},u^{*})}\): \({{{\partial }g_{13}} \over {{\partial }x_i}}=0\), for \(i=1,2,\cdots ,6\).

Second row of the Jacobian matrix \({\nabla _x}{g_3}(x)\mid _{(x^{*},u^{*})}\) for \(i=1,2,\cdots ,6\)

$$\begin{aligned} {{{\partial }g_{23}} \over {{\partial }x_i}}={{{{{\partial }J_{31}} \over {{\partial }x_i}}detJ-J_{31}{{{\partial }detJ} \over {{\partial }x_i}}} \over {detJ^2}} \end{aligned}$$
(35)

Third row of the Jacobian matrix \({\nabla _x}{g_3}(x)\mid _{(x^{*},u^{*})}\): \({{{\partial }g_{33}} \over {{\partial }x_i}}=0\), for \(i=1,2,\cdots ,6\).

Fourth row of the Jacobian matrix \({\nabla _x}{g_3}(x)\mid _{(x^{*},u^{*})}\) for \(i=1,2,\cdots ,6\)

$$\begin{aligned} {{{\partial }g_{43}} \over {{\partial }x_i}}=-{{{{{\partial }J_{32}} \over {{\partial }x_i}}detJ-J_{32}{{{\partial }detJ} \over {{\partial }x_i}}} \over {detJ^2}} \end{aligned}$$
(36)

Fifth row of the Jacobian matrix \({\nabla _x}{g_3}(x)\mid _{(x^{*},u^{*})}\): \({{{\partial }g_{53}} \over {{\partial }x_i}}=0\), for \(i=1,2,\cdots ,6\).

Sixth row of the Jacobian matrix \({\nabla _x}{g_3}(x)\mid _{(x^{*},u^{*})}\) for \(i=1,2,\cdots ,6\)

$$\begin{aligned} {{{\partial }g_{63}} \over {{\partial }x_i}}={{{{{\partial }J_{33}} \over {{\partial }x_i}}detJ-J_{33}{{{\partial }detJ} \over {{\partial }x_i}}} \over {detJ^2}} \end{aligned}$$
(37)

Next, the partial derivatives of the terms \(J_{ij}\) \(i=1,2,3\), \(j=1,2,3\) and of the determinant detJ are computed. It holds that:

$$\begin{aligned}{} & {} {{{\partial }{J}_{11}} \over {{\partial }x_i}}={{\partial }{\tilde{J}_{22}} \over {{\partial }x_i}}{\tilde{J}}_{33}+{\tilde{J}_{22}}{{\partial }{\tilde{J}_{33}} \over {{\partial }x_i}}- {{\partial }{\tilde{J}_{32}} \over {{\partial }x_i}}{\tilde{J}}_{23}-{\tilde{J}_{32}}{{\partial }{\tilde{J}_{23}} \over {{\partial }x_i}} \nonumber \\{} & {} \quad \text {for} \ \ i=1,2,\cdots ,6 \end{aligned}$$
(38)
$$\begin{aligned}{} & {} {{{\partial }{J}_{12}} \over {{\partial }x_i}}={{\partial }{\tilde{J}_{21}} \over {{\partial }x_i}}{\tilde{J}}_{33}+{\tilde{J}_{21}}{{\partial }{\tilde{J}_{33}} \over {{\partial }x_i}}- {{\partial }{\tilde{J}_{31}} \over {{\partial }x_i}}{\tilde{J}}_{23}-{\tilde{J}_{31}}{{\partial }{\tilde{J}_{23}} \over {{\partial }x_i}} \nonumber \\{} & {} \quad \text {for} \ \ i=1,2,\cdots ,6 \end{aligned}$$
(39)
$$\begin{aligned}{} & {} {{{\partial }{J}_{13}} \over {{\partial }x_i}}={{\partial }{\tilde{J}_{21}} \over {{\partial }x_i}}{\tilde{J}}_{32}+{\tilde{J}_{21}}{{\partial }{\tilde{J}_{32}} \over {{\partial }x_i}}- {{\partial }{\tilde{J}_{31}} \over {{\partial }x_i}}{\tilde{J}}_{22}-{\tilde{J}_{31}}{{\partial }{\tilde{J}_{22}} \over {{\partial }x_i}} \nonumber \\{} & {} \quad \text {for} \ \ i=1,2,\cdots ,6 \end{aligned}$$
(40)
$$\begin{aligned}{} & {} {{{\partial }{J}_{21}} \over {{\partial }x_i}}={{\partial }{\tilde{J}_{12}} \over {{\partial }x_i}}{\tilde{J}}_{33}+{\tilde{J}_{12}}{{\partial }{\tilde{J}_{33}} \over {{\partial }x_i}}- {{\partial }{\tilde{J}_{32}} \over {{\partial }x_i}}{\tilde{J}}_{13}-{\tilde{J}_{32}}{{\partial }{\tilde{J}_{13}} \over {{\partial }x_i}} \nonumber \\{} & {} \quad \text {for} \ \ i=1,2,\cdots ,6 \end{aligned}$$
(41)
$$\begin{aligned}{} & {} {{{\partial }{J}_{22}} \over {{\partial }x_i}}={{\partial }{\tilde{J}_{11}} \over {{\partial }x_i}}{\tilde{J}}_{33}+{\tilde{J}_{11}}{{\partial }{\tilde{J}_{33}} \over {{\partial }x_i}}- {{\partial }{\tilde{J}_{31}} \over {{\partial }x_i}}{\tilde{J}}_{13}-{\tilde{J}_{31}}{{\partial }{\tilde{J}_{13}} \over {{\partial }x_i}} \nonumber \\{} & {} \quad \text {for} \ \ i=1,2,\cdots ,6 \end{aligned}$$
(42)
$$\begin{aligned}{} & {} {{{\partial }{J}_{23}} \over {{\partial }x_i}}={{\partial }{\tilde{J}_{11}} \over {{\partial }x_i}}{\tilde{J}}_{32}+{\tilde{J}_{11}}{{\partial }{\tilde{J}_{32}} \over {{\partial }x_i}}- {{\partial }{\tilde{J}_{31}} \over {{\partial }x_i}}{\tilde{J}}_{12}-{\tilde{J}_{31}}{{\partial }{\tilde{J}_{12}} \over {{\partial }x_i}} \nonumber \\{} & {} \quad \text {for} \ \ i=1,2,\cdots ,6 \end{aligned}$$
(43)
$$\begin{aligned}{} & {} {{{\partial }{J}_{31}} \over {{\partial }x_i}}={{\partial }{\tilde{J}_{12}} \over {{\partial }x_i}}{\tilde{J}}_{23}+{\tilde{J}_{12}}{{\partial }{\tilde{J}_{23}} \over {{\partial }x_i}}- {{\partial }{\tilde{J}_{22}} \over {{\partial }x_i}}{\tilde{J}}_{13}-{\tilde{J}_{22}}{{\partial }{\tilde{J}_{13}} \over {{\partial }x_i}} \nonumber \\{} & {} \quad \text {for} \ \ i=1,2,\cdots ,6 \end{aligned}$$
(44)
$$\begin{aligned}{} & {} {{{\partial }{J}_{32}} \over {{\partial }x_i}}={{\partial }{\tilde{J}_{11}} \over {{\partial }x_i}}{\tilde{J}}_{23}+{\tilde{J}_{11}}{{\partial }{\tilde{J}_{23}} \over {{\partial }x_i}}- {{\partial }{\tilde{J}_{21}} \over {{\partial }x_i}}{\tilde{J}}_{13}-{\tilde{J}_{21}}{{\partial }{\tilde{J}_{13}} \over {{\partial }x_i}} \nonumber \\{} & {} \quad \text {for} \ \ i=1,2,\cdots ,6 \end{aligned}$$
(45)
$$\begin{aligned}{} & {} {{{\partial }{J}_{33}} \over {{\partial }x_i}}={{\partial }{\tilde{J}_{11}} \over {{\partial }x_i}}{\tilde{J}}_{22}+{\tilde{J}_{11}}{{\partial }{\tilde{J}_{22}} \over {{\partial }x_i}}- {{\partial }{\tilde{J}_{21}} \over {{\partial }x_i}}{\tilde{J}}_{12}-{\tilde{J}_{21}}{{\partial }{\tilde{J}_{12}} \over {{\partial }x_i}} \nonumber \\{} & {} \quad \text {for} \ \ i=1,2,\cdots ,6 \end{aligned}$$
(46)

Additionally, it holds that for \(i=1,2.\cdots ,6\)

$$\begin{aligned}{} & {} {{{\partial }detJ} \over {{\partial }x_i}}={{{\partial }\tilde{J}_{11}} \over {{\partial }x_i}}({\tilde{J}_{22}}{\tilde{J}_{33}}- {\tilde{J}_{32}}{\tilde{J}_{23}})\nonumber \\{} & {} \quad +{\tilde{J}_{11}} ({{{\partial }\tilde{J}_{22}} \over {{\partial }x_i}}\tilde{J}_{33}+{\tilde{J}_{22}}{{{\partial }\tilde{J}_{33}} \over {{\partial }x_i}}- {{{\partial }\tilde{J}_{32}} \over {{\partial }x_i}}\tilde{J}_{23}-{\tilde{J}_{32}}{{{\partial }\tilde{J}_{23}} \over {{\partial }x_i}} )+\nonumber \\{} & {} \quad {{{\partial }\tilde{J}_{12}} \over {{\partial }x_i}}({\tilde{J}_{21}}{\tilde{J}_{33}}- {\tilde{J}_{31}}{\tilde{J}_{23}})\nonumber \\{} & {} \quad +{\tilde{J}_{12}} ({{{\partial }\tilde{J}_{21}} \over {{\partial }x_i}}\tilde{J}_{33}+{\tilde{J}_{21}}{{{\partial }\tilde{J}_{33}} \over {{\partial }x_i}}- {{{\partial }\tilde{J}_{31}} \over {{\partial }x_i}}\tilde{J}_{23}-{\tilde{J}_{31}}{{{\partial }\tilde{J}_{23}} \over {{\partial }x_i}} )+\nonumber \\{} & {} {{{\partial }\tilde{J}_{13}} \over {{\partial }x_i}}({\tilde{J}_{21}}{\tilde{J}_{32}}- {\tilde{J}_{31}}{\tilde{J}_{22}})\nonumber \\{} & {} \quad +{\tilde{J}_{13}} ({{{\partial }\tilde{J}_{21}} \over {{\partial }x_i}}\tilde{J}_{32}+{\tilde{J}_{21}}{{{\partial }\tilde{J}_{32}} \over {{\partial }x_i}}- {{{\partial }\tilde{J}_{31}} \over {{\partial }x_i}}\tilde{J}_{22}-{\tilde{J}_{31}}{{{\partial }\tilde{J}_{22}} \over {{\partial }x_i}} )+ \end{aligned}$$
(47)

Equivalently, one has for the partial derivatives of \(c_i\), \(i=1,2,3\)

$$\begin{aligned}{} & {} {{{\partial }c_1} \over {{\partial }x_i}}= {{\partial }{\tilde{C}_{11}} \over {{\partial }x_i}}{\dot{\eta }_1}+{\tilde{C}_{11}}{{\partial }{\dot{\eta }_1} \over {{\partial }x_i}}+ {{\partial }{\tilde{C}_{12}} \over {{\partial }x_i}}{\dot{\eta }_2}+{\tilde{C}_{12}}{{\partial }{\dot{\eta }_2} \over {{\partial }x_i}}\nonumber \\{} & {} \quad + {{\partial }{\tilde{C}_{13}} \over {{\partial }x_i}}{\dot{\eta }_3}+{\tilde{C}_{13}}{{\partial }{\dot{\eta }_3} \over {{\partial }x_i}} \ \ \text {for} \ i=1,2\cdots ,6 \end{aligned}$$
(48)
$$\begin{aligned}{} & {} \quad {{{\partial }c_2} \over {{\partial }x_i}}= {{\partial }{\tilde{C}_{21}} \over {{\partial }x_i}}{\dot{\eta }_1}+{\tilde{C}_{21}}{{\partial }{\dot{\eta }_1} \over {{\partial }x_i}}+ {{\partial }{\tilde{C}_{22}} \over {{\partial }x_i}}{\dot{\eta }_2}+{\tilde{C}_{22}}{{\partial }{\dot{\eta }_2} \over {{\partial }x_i}}\nonumber \\{} & {} \quad + {{\partial }{\tilde{C}_{23}} \over {{\partial }x_i}}{\dot{\eta }_3}+{\tilde{C}_{23}}{{\partial }{\dot{\eta }_3} \over {{\partial }x_i}}\ \ \text {for} \ i=1,2\cdots ,6 \end{aligned}$$
(49)
$$\begin{aligned}{} & {} {{{\partial }c_3} \over {{\partial }x_i}}= {{\partial }{\tilde{C}_{31}} \over {{\partial }x_i}}{\dot{\eta }_1}+{\tilde{C}_{31}}{{\partial }{\dot{\eta }_1} \over {{\partial }x_i}}+ {{\partial }{\tilde{C}_{32}} \over {{\partial }x_i}}{\dot{\eta }_2}+{\tilde{C}_{32}}{{\partial }{\dot{\eta }_2} \over {{\partial }x_i}}\nonumber \\{} & {} \quad + {{\partial }{\tilde{C}_{33}} \over {{\partial }x_i}}{\dot{\eta }_3}+{\tilde{C}_{33}}{{\partial }{\dot{\eta }_3} \over {{\partial }x_i}}\ \ \text {for} \ i=1,2\cdots ,6 \end{aligned}$$
(50)

Additionally, one has for the partial derivatives of \(c_i\), \(i=1,2,3\)

$$\begin{aligned}{} & {} {{{\partial }q_1} \over {{\partial }x_i}}= {{\partial }{\tilde{F}_{11}} \over {{\partial }x_i}}{\dot{\eta }_1}+{\tilde{F}_{11}}{{\partial }{\dot{\eta }_1} \over {{\partial }x_i}}+ {{\partial }{\tilde{F}_{12}} \over {{\partial }x_i}}{\dot{\eta }_2}+{\tilde{F}_{12}}{{\partial }{\dot{\eta }_2} \over {{\partial }x_i}}\nonumber \\{} & {} \quad + {{\partial }{\tilde{F}_{13}} \over {{\partial }x_i}}{\dot{\eta }_3}+{\tilde{F}_{13}}{{\partial }{\dot{\eta }_3} \over {{\partial }x_i}} \ \ \text {for} \ i=1,2\cdots ,6 \end{aligned}$$
(51)
$$\begin{aligned}{} & {} \quad {{{\partial }q_2} \over {{\partial }x_i}}= {{\partial }{\tilde{F}_{21}} \over {{\partial }x_i}}{\dot{\eta }_1}+{\tilde{F}_{21}}{{\partial }{\dot{\eta }_1} \over {{\partial }x_i}}+ {{\partial }{\tilde{F}_{22}} \over {{\partial }x_i}}{\dot{\eta }_2}+{\tilde{F}_{22}}{{\partial }{\dot{\eta }_2} \over {{\partial }x_i}}\nonumber \\{} & {} \quad + {{\partial }{\tilde{F}_{23}} \over {{\partial }x_i}}{\dot{\eta }_3}+{\tilde{F}_{23}}{{\partial }{\dot{\eta }_3} \over {{\partial }x_i}}\ \ \text {for} \ i=1,2\cdots ,6 \end{aligned}$$
(52)
$$\begin{aligned}{} & {} {{{\partial }q_3} \over {{\partial }x_i}}= {{\partial }{\tilde{F}_{31}} \over {{\partial }x_i}}{\dot{\eta }_1}+{\tilde{F}_{31}}{{\partial }{\dot{\eta }_1} \over {{\partial }x_i}}+ {{\partial }{\tilde{F}_{32}} \over {{\partial }x_i}}{\dot{\eta }_2}+{\tilde{F}_{32}}{{\partial }{\dot{\eta }_2} \over {{\partial }x_i}}\nonumber \\{} & {} \quad + {{\partial }{\tilde{F}_{33}} \over {{\partial }x_i}}{\dot{\eta }_3}+{\tilde{F}_{33}}{{\partial }{\dot{\eta }_3} \over {{\partial }x_i}}\ \ \text {for} \ i=1,2\cdots ,6 \end{aligned}$$
(53)

Moreover, about the partial derivatives of variables \(\eta _i\), \(i=1,2,3\) one has that

$$\begin{aligned}{} & {} {{{\partial }\dot{\eta }_1} \over {{\partial }x_2}}=1 &{{{\partial }\dot{\eta }_1} \over {{\partial }x_i}}=0 \ \ \text {for} \ i=1,3,4,5,6 \end{aligned}$$
(54)
$$\begin{aligned}{} & {} {{{\partial }\dot{\eta }_2} \over {{\partial }x_4}}=1 &{{{\partial }\dot{\eta }_2} \over {{\partial }x_i}}=0 \ \ \text {for} \ i=1,2,3,5,6 \end{aligned}$$
(55)
$$\begin{aligned}{} & {} {{{\partial }\dot{\eta }_3} \over {{\partial }x_6}}=1 &{{{\partial }\dot{\eta }_3} \over {{\partial }x_i}}=0 \ \ \text {for} \ i=1,2,3,4,5 \end{aligned}$$
(56)

Next, one proceeds to the computation of the partial derivatives \({{{\partial }\tilde{J}_{ij}} \over {{\partial }x_i}}\) \(i=1,2,3\), \(j=1,2,3\), \({{{\partial }\tilde{C}_{ij}} \over {{\partial }x_i}}\) \(i=1,2,3\), \(j=1,2,3\) and \({{{\partial }\tilde{F}_{ij}} \over {{\partial }x_i}}\) \(i=1,2,3\), \(j=1,2,3\). It holds that:

$$\begin{aligned}{} & {} {{{\partial }\tilde{J}_{11}} \over {{\partial }x_i}}=0\text { for }i=1,2,3,4,6\text { and }{{{\partial }\tilde{J}_{11}} \over {{\partial }x_5}}\\{} & {} \quad =-2{m_{11}}cos(x_5)sin(x_5)+2m_{22}sin(x_5)cos(x_5). \\{} & {} {{{\partial }\tilde{J}_{12}} \over {{\partial }x_i}}=0\text { for }i=1,2,3,4,6\text { and }{{{\partial }\tilde{J}_{12}} \over {{\partial }x_5}}\\{} & {} \quad =(m_{11}-m_{22})[cos^2(x_5)-sin^2(x_5)]. \\{} & {} \quad {{{\partial }\tilde{J}_{13}} \over {{\partial }x_i}}=0\text { for }i=1,2,3,4,6\text { and }{{{\partial }\tilde{J}_{13}} \over {{\partial }x_5}}\\{} & {} \quad =-m_{23}cos(x_5).\\{} & {} {{{\partial }\tilde{J}_{21}} \over {{\partial }x_i}}=0\text { for }i=1,2,3,4,6\text { and }{{{\partial }\tilde{J}_{21}} \over {{\partial }x_5}}\\{} & {} \quad =-(m_{11}-m_{22})[cos^2(x_5)-sin^2(x_5)]. \\{} & {} {{{\partial }\tilde{J}_{22}} \over {{\partial }x_i}}=0\text { for }i=1,2,3,4,6\text { and }{{{\partial }\tilde{J}_{22}} \over {{\partial }x_5}}\\{} & {} \quad =2{m_{11}}sin(x_5)cos(x_5)-2{m_{22}}sin(x_5)cos(x_5). \\{} & {} {{{\partial }\tilde{J}_{23}} \over {{\partial }x_i}}=0\text { for }i=1,2,3,4,6\text { and }{{{\partial }\tilde{J}_{23}} \over {{\partial }x_5}}\\{} & {} \quad =-{m_{23}}sin(x_5). \\{} & {} \quad {{{\partial }\tilde{J}_{31}} \over {{\partial }x_i}}=0\text { for }i=1,2,3,4,6\text { and }{{{\partial }\tilde{J}_{31}} \over {{\partial }x_5}}\\{} & {} \quad =-{m_{23}}cos(x_5). \\{} & {} \quad {{{\partial }\tilde{J}_{32}} \over {{\partial }x_i}}=0\text { for }i=1,2,3,4,6\text { and }{{{\partial }\tilde{J}_{32}} \over {{\partial }x_5}}\\{} & {} \quad =-{m_{23}}cos(x_5). \\{} & {} {{{\partial }\tilde{J}_{33}} \over {{\partial }x_i}}=0\text { for }i=1,2,3,4,4,6 \end{aligned}$$

Furthermore, it holds that

$$\begin{aligned}{} & {} {{{\partial }\tilde{F}_{11}} \over {{\partial }x_i}}=0\text { for }i=1,2,3,4,6\text { and }{{{\partial }\tilde{F}_{11}} \over {{\partial }x_5}}\\{} & {} \quad =-2{d_{11}}cos(x_5)sin(x_5)+2{d_{22}}sin(x_5)cos(x_5). \\{} & {} \quad {{{\partial }\tilde{F}_{12}} \over {{\partial }x_i}}=0\text { for }i=1,2,3,4,6\text { and }{{{\partial }\tilde{F}_{12}} \over {{\partial }x_5}}\\{} & {} \quad =(d_{11}-d_{22})[cos^2(x_5)-sin^2(x_5)]. \\{} & {} \quad {{{\partial }\tilde{F}_{13}} \over {{\partial }x_i}}=0\text { for }i=1,2,3,4,6\text { and }{{{\partial }\tilde{F}_{13}} \over {{\partial }x_5}}\\{} & {} \quad =-d_{23}cos(x_5). \\{} & {} {{{\partial }\tilde{F}_{21}} \over {{\partial }x_i}}=0\text { for }i=1,2,3,4,6\text { and }{{{\partial }\tilde{F}_{21}} \over {{\partial }x_5}}\\{} & {} \quad =(d_{11}-d_{22})[cos^2(x_5)-sin^2(x_5)]. \\{} & {} \quad {{{\partial }\tilde{F}_{22}} \over {{\partial }x_i}}=0\text { for }i=1,2,3,4,6\text { and }{{{\partial }\tilde{F}_{22}} \over {{\partial }x_5}}\\{} & {} \quad =2{d_{11}}sin(x_5)cos(x_5)-2{d_{22}}sin(x_5)cos(x_5). \\{} & {} \quad {{{\partial }\tilde{F}_{23}} \over {{\partial }x_i}}=0\text { for }i=1,2,3,4,6\text { and }{{{\partial }\tilde{F}_{23}} \over {{\partial }x_5}}\\{} & {} \quad =-{d_{22}}sin(x_5). \\{} & {} \quad {{{\partial }\tilde{F}_{31}} \over {{\partial }x_i}}=0\text { for }i=1,2,3,4,6\text { and }{{{\partial }\tilde{F}_{31}} \over {{\partial }x_5}}\\{} & {} \quad =-{d_{32}}cos(x_5). \\{} & {} \quad {{{\partial }\tilde{F}_{32}} \over {{\partial }x_i}}=0\text { for }i=1,2,3,4,6\text { and }{{{\partial }\tilde{F}_{32}} \over {{\partial }x_5}}\\{} & {} \quad =-{d_{32}}sin(x_5). \\{} & {} {{{\partial }\tilde{F}_{33}} \over {{\partial }x_i}}=0\text { for }i=1,2,3,4,5,6. \end{aligned}$$

3.2 Stabilizing feedback control

After linearization around its current operating point \((x^{*},u^{*})\), the dynamic model of the Unmanned Surface Vessel (USV) is written as [1]

$$\begin{aligned} \dot{x}=Ax+Bu+d_1 \end{aligned}$$
(57)

Parameter \(d_1\) stands for the linearization error in the USV’s model appearing previously in Eq. (57). The reference setpoints for the USV’s state vector are denoted by \(\mathbf{{x_d}}=[x_1^{d},\cdots ,x_6^{d}]\). Tracking of this trajectory is achieved after applying the control input \(u^{*}\). At every time instant the control input \(u^{*}\) is assumed to differ from the control input u appearing in Eq. (57) by an amount equal to \({\Delta }u\), that is \(u^{*}=u+{\Delta }u\)

$$\begin{aligned} \dot{x}_d=Ax_d+Bu^{*}+d_2 \end{aligned}$$
(58)

The dynamics of the controlled system described in Eq. (57) can be also written as

$$\begin{aligned} \dot{x}=Ax+Bu+Bu^{*}-Bu^{*}+d_1 \end{aligned}$$
(59)

and by denoting \(d_3=-Bu^{*}+d_1\) as an aggregate disturbance term one obtains

$$\begin{aligned} \dot{x}=Ax+Bu+Bu^{*}+d_3 \end{aligned}$$
(60)

By subtracting Eq. (58) from Eq. (60) one has

$$\begin{aligned} \dot{x}-\dot{x}_d=A(x-x_d)+Bu+d_3-d_2 \end{aligned}$$
(61)

By denoting the tracking error as \(e=x-x_d\) and the aggregate disturbance term as \(L{\cdot }\tilde{d}=d_3-d_2\), the tracking error dynamics becomes

$$\begin{aligned} \dot{e}=Ae+Bu+L\tilde{d} \end{aligned}$$
(62)

where L is the disturbance inputs gain matrix. For the approximately linearized model of the system a stabilizing feedback controller is developed. The controller has the form

$$\begin{aligned} u(t)=-Ke(t) \end{aligned}$$
(63)

with \(K={1 \over r}{B^T}P\) where P is a positive definite symmetric matrix which is obtained from the solution of the Riccati equation [1]

$$\begin{aligned} {A^T}P+PA+Q-P({2 \over r}B{B^T}-{1 \over {\rho ^2}}L{L^T})P=0 \end{aligned}$$
(64)

where Q is a positive semi-definite symmetric matrix. The diagram of the considered control loop for the USV is depicted in Fig. 2.

Fig. 2
figure 2

Diagram of the control scheme for the Unmanned Surface Vessel

Full size image

4 Lyapunov stability analysis

4.1 Stability proof

Through Lyapunov stability analysis it will be shown that the proposed nonlinear control scheme assures \(H_{\infty }\) tracking performance for the Unmanned Surface Vessel (USV), and that in case of bounded disturbance terms asymptotic convergence to the reference setpoints is achieved [1, 37]. The tracking error dynamics for the USV is written in the form

$$\begin{aligned} \dot{e}=Ae+Bu+L\tilde{d} \end{aligned}$$
(65)

where in the 3-DOF USV’s case \(L=I{\in }R^6\) with I being the identity matrix. Variable \(\tilde{d}\) denotes model uncertainties and external disturbances of the USV’s model. The following Lyapunov equation is considered

$$\begin{aligned} V={1 \over 2}{e^T}Pe \end{aligned}$$
(66)

where \(e=x-x_d\) is the tracking error. By differentiating with respect to time one obtains

$$\begin{aligned}{} & {} \dot{V}={1 \over 2}{\dot{e}^T}Pe+{1 \over 2}{e^T}P\dot{e}{\Rightarrow }\nonumber \\{} & {} \quad \dot{V}={1 \over 2}{[Ae+Bu+L\tilde{d}]^T}Pe+{1 \over 2}{e^T}\nonumber \\{} & {} \quad P[Ae+Bu+L\tilde{d}]{\Rightarrow } \end{aligned}$$
(67)
$$\begin{aligned}{} & {} \dot{V}={1 \over 2}[{e^T}{A^T}+{u^T}{B^T}+{\tilde{d}^T}{L^T}]Pe+\nonumber \\{} & {} \quad +{1 \over 2}{e^T}P[Ae+Bu+L\tilde{d}]{\Rightarrow } \end{aligned}$$
(68)
$$\begin{aligned}{} & {} \dot{V}={1 \over 2}{e^T}{A^T}Pe+{1 \over 2}{u^T}{B^T}Pe+{1 \over 2}{\tilde{d}^T}{L^T}Pe+\nonumber \\{} & {} \quad {1 \over 2}{e^T}PAe+{1 \over 2}{e^T}PBu+{1 \over 2}{e^T}PL\tilde{d} \end{aligned}$$
(69)

The previous equation is rewritten as

$$\begin{aligned}{} & {} \dot{V}={1 \over 2}{e^T}({A^T}P+PA)e+({1 \over 2}{u^T}{B^T}Pe+{1 \over 2}{e^T}PBu)+\nonumber \\{} & {} \quad +({1 \over 2}{\tilde{d}^T}{L^T}Pe+{1 \over 2}{e^T}PL\tilde{d}) \end{aligned}$$
(70)

Assumption: For given positive definite matrix Q and coefficients r and \(\rho\) there exists a positive definite matrix P, which is the solution of the following matrix equation

$$\begin{aligned} {A^T}P+PA=-Q+P({2 \over r}B{B^T}-{1 \over \rho ^2}L{L^T})P \end{aligned}$$
(71)

Moreover, the following feedback control law is applied to the system

$$\begin{aligned} u=-{1 \over {r}}{B^T}Pe \end{aligned}$$
(72)

By substituting Eq. (71) and Eq. (72) one obtains

$$\begin{aligned}{} & {} \dot{V}={1 \over 2}{e^T}[-Q+P({2 \over r}B{B^T}-{1 \over \rho ^2}L{L^T})P]e+\nonumber \\{} & {} \quad +{e^T}PB(-{1 \over {r}}{B^T}Pe)+{e^T}PL\tilde{d}{\Rightarrow } \end{aligned}$$
(73)
$$\begin{aligned}{} & {} \dot{V}=-{1 \over 2}{e^T}Qe+{1 \over {r}}{e^T}PB{B^T}Pe-{1 \over {2\rho ^2}}{e^T}PL{L^T}Pe\nonumber \\{} & {} \quad -{1 \over {r}}{e^T}PB{B^T}Pe+{e^T}PL\tilde{d} \end{aligned}$$
(74)

which after intermediate operations gives

$$\begin{aligned} \dot{V}=-{1 \over 2}{e^T}Qe-{1 \over {2\rho ^2}}{e^T}PL{L^T}Pe+{e^T}PL\tilde{d} \end{aligned}$$
(75)

or, equivalently

$$\begin{aligned}{} & {} \dot{V}=-{1 \over 2}{e^T}Qe-{1 \over {2\rho ^2}}{e^T}PL{L^T}Pe+\nonumber \\{} & {} \quad +{1 \over 2}{e^T}PL\tilde{d}+{1 \over 2}{\tilde{d}^T}{L^T}Pe \end{aligned}$$
(76)

Lemma

: The following inequality holds

$$\begin{aligned} {1 \over 2}{e^T}L\tilde{d}+{1 \over 2}\tilde{d}{L^T}Pe-{1 \over {2\rho ^2}}{e^T}PL{L^T}Pe{\le }{1 \over 2}{\rho ^2}{\tilde{d}^T}\tilde{d} \end{aligned}$$
(77)

Proof

: The binomial \(({\rho }{\alpha }-{1 \over \rho }b)^2\) is considered. Expanding the left part of the above inequality one gets

$$\begin{aligned}{} & {} {\rho ^2}{a^2}+{1 \over {\rho ^2}}{b^2}-2ab \ge 0 \Rightarrow {1 \over 2}{\rho ^2}{a^2}+{1 \over {2\rho ^2}}{b^2}-ab \ge 0 \Rightarrow \nonumber \\{} & {} \quad ab-{1 \over {2\rho ^2}}{b^2} \le {1 \over 2}{\rho ^2}{a^2} \Rightarrow {1 \over 2}ab+{1 \over 2}ab-{1 \over {2\rho ^2}}{b^2} \le {1 \over 2}{\rho ^2}{a^2} \end{aligned}$$
(78)

The following substitutions are carried out: \(a=\tilde{d}\) and \(b={e^T}{P}L\) and the previous relation becomes

$$\begin{aligned} {1 \over 2}{\tilde{d}^T}{L^T}Pe+{1 \over 2}{e^T}PL\tilde{d}-{1 \over {2\rho ^2}}{e^T}PL{L^T}Pe{\le }{1 \over 2}{\rho ^2}\tilde{d}^T\tilde{d} \end{aligned}$$
(79)

Eq. (79) is substituted in Eq. (76) and the inequality is enforced, thus giving

$$\begin{aligned} \dot{V}{\le }-{1 \over 2}{e^T}Qe+{1 \over 2}{\rho ^2}{\tilde{d}^T}\tilde{d} \end{aligned}$$
(80)

Eq. (80) shows that the \(H_{\infty }\) tracking performance criterion is satisfied. The integration of \(\dot{V}\) from 0 to T gives

$$\begin{aligned}{} & {} {\int _0^T}\dot{V}(t)dt{\le }-{1 \over 2}{\int _0^T}{||e||_Q^2}{dt}+{1 \over 2}{\rho ^2}{\int _0^T}{||\tilde{d}||^2}{dt}{\Rightarrow }\nonumber \\{} & {} \quad 2V(T)+{\int _0^T}{||e||_Q^2}{dt}{\le }2V(0)+{\rho ^2}{\int _0^T}{||\tilde{d}||^2}dt \end{aligned}$$
(81)

Moreover, if there exists a positive constant \(M_d>0\) such that

$$\begin{aligned} \int _0^{\infty }{||\tilde{d}||^2}dt \le M_d \end{aligned}$$
(82)

then one gets

$$\begin{aligned} {\int _0^{\infty }}{||e||_Q^2}dt \le 2V(0)+{\rho ^2}{M_d} \end{aligned}$$
(83)

Thus, the integral \({\int _0^{\infty }}{||e||_Q^2}dt\) is bounded. Moreover, V(T) is bounded and from the definition of the Lyapunov function V in Eq. (66) it becomes clear that e(t) will be also bounded since \(e(t) \ \in \ \Omega _e=\{e|{e^T}Pe{\le }2V(0)+{\rho ^2}{M_d}\}\). According to the above and with the use of Barbalat’s Lemma one obtains \(\lim _{t \rightarrow \infty }{e(t)}=0\).

The outline of the global stability proof is that at each iteration of the control algorithm the state vector of the fully actuated USV converges towards the temporary operating point and the temporary operating point in turn converges towards the reference trajectory. Thus, the control scheme exhibits global asymptotic stability properties and not local stability. Assume the i-th iteration of the control algorithm and the i-th time interval about which a positive definite symmetric matrix P is obtained from the solution of the Riccati Equation appearing in Eq. (71). By following the stages of the stability proof one arrives at Eq. (80) which shows that the H-infinity tracking performance criterion holds. By selecting the attenuation coefficient \(\rho\) to be sufficiently small and in particular to satisfy \(\rho ^2<||e||^2_Q / ||\tilde{d}||^2\) one has that the first derivative of the Lyapunov function is upper bounded by 0. Therefore for the i-th time interval it is proven that the Lyapunov function defined in Eq (66) is a decreasing one. This signifies that between the beginning and the end of the i-th time interval there will be a drop of the value of the Lyapunov function and since matrix P is a positive definite one, the only way for this to happen is the Euclidean norm of the state vector error e to be decreasing. This means that comparing to the beginning of each time interval, the distance of the state vector error from 0 at the end of the time interval has diminished. Consequently as the iterations of the control algorithm advance the tracking error will approach zero, and this is a global asymptotic stability condition.

4.1.1 Robust state estimation with Kalman filtering

The control loop has to be implemented with the use of information provided by a small number of sensors and by processing only a small number of state variables. To reconstruct the missing information about the state vector of the Unmanned Surface Vessel (USV) it is proposed to use a filtering scheme and based on it to apply state estimation-based control [1, 37]. By denoting as A(k), B(k) and C(k) the discrete-time equivalents of matrices A, B and C of the linearized state-space model of the system, the recursion of the \(H_{\infty }\) Kalman Filter, for the 3-DOF USV, can be formulated in terms of a measurement update and a time update part

Measurement update:

$$\begin{aligned}{} & {} D(k)=[I-{\theta }W(k)P^{-}(k)+{C^T}(k)R(k)^{-1}C(k)P^{-}(k)]^{-1}\nonumber \\{} & {} \quad K(k)=P^{-}(k)D(k){C^T}(k)R(k)^{-1}\nonumber \\{} & {} \quad \hat{x}(k)=\hat{x}^{-}(k)+K(k)[y(k)-C\hat{x}^{-}(k)] \end{aligned}$$
(84)

Time update:

$$\begin{aligned}{} & {} \hat{x}^{-}(k+1)=A(k)x(k)+B(k)u(k)\nonumber \\{} & {} \quad P^{-}(k+1)=A(k)P^{-}(k)D(k)A^T(k)+Q(k) \end{aligned}$$
(85)

where it is assumed that parameter \(\theta\) is sufficiently small to assure that the covariance matrix \({P^{-}(k)}^{-1}-{\theta }W(k)+C^T(k)R(k)^{-1}C(k)\) will be positive definite. When \(\theta =0\) the \(H_{\infty }\) Kalman Filter becomes equivalent to the standard Kalman Filter. One can measure only a part of the state vector of the 3-DOF USV, for instance state variables \(x_1\) (position of x-axis), \(x_2\) (position on the y-axis), \(x_5\) (heading angle of the USV) and \(x_6\) (rate of change of heading angle), and can estimate through filtering the rest of the state vector elements. Moreover, the proposed Kalman filtering method can be used for sensor fusion purposes.

5 Simulation tests

The performance of the proposed nonlinear optimal control scheme for the USV is further confirmed through simulation experiments. The model of the unmanned surface vessel that has been considered in these tests is the one which has been used in [3]. Certainly, the results can be generalized for underactuated USVs. The sampling period was \(T_s=0.01\)sec. To compute the control inputs of the USV, the algebraic Riccati equation of Eq. (71) had to be solved at each time-step of the control algorithm. The tracking performance of the control scheme was tested using several reference paths that the USV had to follow. The obtained results are depicted in Figs. 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17 and 18. The real values of the USV’s state vector are printed in blue, the estimated values which were provided by the H-infinity Kalman Filter are plotted in green, while the related reference setpoints are shown in red. Distance is measured in meters (m), angles in rad and time in sec. It can be noticed that in all cases, the proposed H-infinity controller achieved fast and accurate tracking of the reference paths under moderate variations of the control inputs.

Fig. 3
figure 3

Tracking of setpoint 1 for the 3-DOF USV a convergence of state variables \(x_1\) to \(x_3\) to their reference setpoints (red line: setpoint, blue line: real value, green line: estimated value), b convergence of state variables \(x_4\) to \(x_6\) to their reference setpoints (red line: setpoint, blue line: real value, green line: estimated value)

Full size image
Fig. 4
figure 4

Tracking of setpoint 1 for the 3-DOF USV a control inputs \(u_1\) to \(u_3\) applied to the propulsion and steering system of the USV, b tracking of the reference path (red line) by the USV (blue line) in the xy plane

Full size image
Fig. 5
figure 5

Tracking of setpoint 2 for the 3-DOF USV a convergence of state variables \(x_1\) to \(x_3\) to their reference setpoints (red line: setpoint, blue line: real value, green line: estimated value), b convergence of state variables \(x_4\) to \(x_6\) to their reference setpoints (red line: setpoint, blue line: real value, green line: estimated value)

Full size image
Fig. 6
figure 6

Tracking of setpoint 2 for the 3-DOF USV a control inputs \(u_1\) to \(u_3\) applied to the propulsion and steering system of the USV, b tracking of the reference path (red line) by the USV (blue line) in the xy plane

Full size image
Fig. 7
figure 7

Tracking of setpoint 3 for the 3-DOF USV a convergence of state variables \(x_1\) to \(x_3\) to their reference setpoints (red line: setpoint, blue line: real value, green line: estimated value), b convergence of state variables \(x_4\) to \(x_6\) to their reference setpoints (red line: setpoint, blue line: real value, green line: estimated value)

Full size image
Fig. 8
figure 8

Tracking of setpoint 3 for the 3-DOF USV a control inputs \(u_1\) to \(u_3\) applied to the propulsion and steering system of the USV, b tracking of the reference path (red line) by the USV (blue line) in the xy plane

Full size image
Fig. 9
figure 9

Tracking of setpoint 4 for the 3-DOF USV a convergence of state variables \(x_1\) to \(x_3\) to their reference setpoints (red line: setpoint, blue line: real value, green line: estimated value), b convergence of state variables \(x_4\) to \(x_6\) to their reference setpoints (red line: setpoint, blue line: real value, green line: estimated value)

Full size image
Fig. 10
figure 10

Tracking of setpoint 4 for the 3-DOF USV a control inputs \(u_1\) to \(u_3\) applied to the propulsion and steering system of the USV, b tracking of the reference path (red line) by the USV (blue line) in the xy plane

Full size image
Fig. 11
figure 11

Tracking of setpoint 5 for the 3-DOF USV a convergence of state variables \(x_1\) to \(x_3\) to their reference setpoints (red line: setpoint, blue line: real value, green line: estimated value), b convergence of state variables \(x_4\) to \(x_6\) to their reference setpoints (red line: setpoint, blue line: real value, green line: estimated value)

Full size image
Fig. 12
figure 12

Tracking of setpoint 5 for the 3-DOF USV a control inputs \(u_1\) to \(u_3\) applied to the propulsion and steering system of the USV, b tracking of the reference path (red line) by the USV (blue line) in the xy plane

Full size image
Fig. 13
figure 13

Tracking of setpoint 6 for the 3-DOF USV a convergence of state variables \(x_1\) to \(x_3\) to their reference setpoints (red line: setpoint, blue line: real value, green line: estimated value), b convergence of state variables \(x_4\) to \(x_6\) to their reference setpoints (red line: setpoint, blue line: real value, green line: estimated value)

Full size image
Fig. 14
figure 14

Tracking of setpoint 6 for the 3-DOF USV a control inputs \(u_1\) to \(u_3\) applied to the propulsion and steering system of the USV, b tracking of the reference path (red line) by the USV (blue line) in the xy plane

Full size image
Fig. 15
figure 15

Tracking of setpoint 7 for the 3-DOF USV a convergence of state variables \(x_1\) to \(x_3\) to their reference setpoints (red line: setpoint, blue line: real value, green line: estimated value), b convergence of state variables \(x_4\) to \(x_6\) to their reference setpoints (red line: setpoint, blue line: real value, green line: estimated value)

Full size image
Fig. 16
figure 16

Tracking of setpoint 7 for the 3-DOF USV a control inputs \(u_1\) to \(u_3\) applied to the propulsion and steerin system of the USV, b tracking of the reference path (red line) by the USV (blue line) in the xy plane

Full size image
Fig. 17
figure 17

Tracking of setpoint 8 for the 3-DOF USV a convergence of state variables \(x_1\) to \(x_3\) to their reference setpoints (red line: setpoint, blue line: real value, green line: estimated value), b convergence of state variables \(x_4\) to \(x_6\) to their reference setpoints (red line: setpoint, blue line: real value, green line: estimated value)

Full size image
Fig. 18
figure 18

Tracking of setpoint 8 for the 3-DOF USV a control inputs \(u_1\) to \(u_3\) applied to the propulsion and steering system of the USV, b tracking of the reference path (red line) by the USV (blue line) in the xy plane

Full size image

It is noted that the transient performance of the control method depends of the selection of parameters r, \(\rho\) and Q which appear in the Riccati equation of Eq. (71). It can noticed that relatively small values of r result in elimination of the tracking error while relatively large values of matrix Q result in fast convergence to the reference setpoint. Moreover, coefficient \(\rho\) affects the robustness of the control loop. The smallest value of \(\rho\) for which one can obtain a valid solution from the algebraic Riccati equation of Eq. (71) is the one that provides the control loop with maximum robustness. It is also pointed out that the use of the H-infinity Kalman Filter as a robust state estimator has allowed to implement feedback control by using measurement of only the cartesian coordinates of the USV (x, y), of its heading angle \(\psi\), and of the heading angle’s change rate \(\dot{\psi }\).

The advantages from the application of the proposed nonlinear optimal control method are outlined as follows: (i) unlike global linearization-based control schemes, the proposed nonlinear optimal control method does not require changes of variables (diffeomorphisms) and application of complicated transformations of the system’s state-space model (ii) the new optimal control method is applied directly on the initial nonlinear model of the 3-DOF USV and avoids inverse transformations which are met in global linearization-based control and which may come against singularities, (iii) for the case of multivariable electromechanical systems, as for instance the USV, finding global linearization transformations is a non-trivial problem which the proposed control method avoids (iv) unlike NMPC approaches, the proposed control method is of proven convergence and stability, (v) unlike sliding-mode control approaches, the proposed control method does not rely on intuitive definition of sliding surfaces and does not need prior transformation of the state-space model into the canonical form. (vi) unlike backstepping control, the nonlinear optmal control method does not require the state-space model of the USV to be found in the backstepping integral (triangular) form, (vii) unlike PID control, the selection of the control gains of the nonlinear optimal control scheme avoids heuristics while the reliable performance of the control loop is ensured at changes of operating points (viii) unlike multiple local models-based linearization methods, in the nonlinear optimal control approach there is no need to define multiple linearization points or to solve multiple Riccati equations and Linear Matrix Inequalities (LMIs).Consequently, the present article’s control scheme is computationally more efficient.

To elaborate on the tracking performance and on the robustness of the proposed nonlinear optimal control method for the dynamics of the 3-DOF unmanned surface vessel the following Tables are given: (i) Table 1 which provides information about the accuracy of tracking of the reference setpoints by the state variables of the 3-DOF USV’s state-space model, (ii) Table 2 which provides information about the robustness of the control method to parametric changes in the model of the 3-DOF USV’s dynamics (change in the USV’s inertia matrix coefficient \(m_{33}\)) , (iii) Table 3 which provides information about the precision in state variables’ estimation that is achieved by the H-infinity Kalman Filter, (iv) Table 4 which provides the convergence times of the 3-DOF USV’s state variables to the associated setpoints.

Table 1 Tracking RMSE for the 3-DOF USV in the disturbance-free case \({\cdot }10^{-3}\)
Full size table
Table 2 Tracking RMSE for the 3-DOF USV in the case of disturbances \({\cdot }10^{-3}\)
Full size table
Table 3 RMSE for the estimation performed by the H-infinity KF \({\cdot }10^{-4}\)
Full size table
Table 4 Convergence time (sec) for the 3-DOF USV’s state variables
Full size table

Finally, the global stability propoerties of the nonlinear optimal control method for the dynamic model of the 3-DOF USV are further confirmed through the following diagrams in Figs. 19, 20, 21 and 22 depicting the variations of the control system’s Lyapunov function.

Fig. 19
figure 19

Variations of the Lyapunov function of the 3-DOF USV’s control loop a when tracking setpoint 1, b when tracking setpoint 2

Full size image
Fig. 20
figure 20

Variations of the Lyapunov function of the 3-DOF USV’s control loop a when tracking setpoint 3, b when tracking setpoint 4

Full size image
Fig. 21
figure 21

Variations of the Lyapunov function of the 3-DOF USV’s control loop a when tracking setpoint 5, b when tracking setpoint 6

Full size image
Fig. 22
figure 22

Variations of the Lyapunov function of the 3-DOF USV’s control loop a when tracking setpoint 7, b when tracking setpoint 8

Full size image

6 Conclusions

Autonomous navigation for Unmanned Surface Vessels (USVs) depends on the solution of the related nonlinear control problem. To this end, a new nonlinear optimal control method for 3-DOF USVs has been proposed. The method relies on approximate linearization of the dynamic model of the 3-DOF USVs through Taylor series expansion and the computation of the associated Jacobian matrices. This linearization process takes place at each iteration of the control algorithm, around a temporary operating point. The operating point is dynamically updated at each sampling period and is defined by the present value of the system’s state vector and by the last sampled value of the control inputs vector. The modelling error which is due to truncation of higher-order terms from the Taylor series expansion is treated as a perturbation which is dynamically updated by the robustness of the control algorithm. For the approximately linearized model of the system, a stabilizing H-infinity (optimal) feedback controller is designed.

The proposed H-infinity feedback controller achieves the solution of the optimal control problem for the dynamic model of the 3-DOF USV under model uncertainty and external perturbations. Actually, it represents the solution of a min-max differential game in which the controller tries to minimize a quadratic cost function of the state vector’s tracking error, whereas the model uncertainty and external perturbation terms try to maximize this cost function. To compute the stabilizing feedback gains of the H-infinity controller, an algebraic Riccati equation is repetitively solved at each time-step of the control algorithm. The global asymptotic stability properties of the control scheme are proven through Lyapunov analysis. Furthermore, to implement state estimation-based control without the need to measure the entire state vector of the 3-DOF USV, the H-infinity Kalman Filter is used as a robust state estimator. The proposed nonlinear optimal control method for USVs retains the known advantages of linear optimal control, that is fast and accurate tracking of reference setpoints under moderate variations of the control inputs.

The solution of the nonlinear optimal control problem for unmanned vessels allows for keeping small the ranges of the vessel’s control inputs. Consequently, it allows for reducing energy consumption and thus it permits to improve the vessels’ autonomy and operational capacity.The article’s nonlinear optimal control method is suitable for all types of drones and autonomous vehicles, that is UGVs, AUVs, USVs and AUVs. The aim is to extend the use of the proposed nonlinear optimal control method to more types unmanned surface vessels and to more types of autonomous underwater vessels. So far the method has been used in 3-DOF unmanned surface vessels, 3-DOF autonomous underactuated hovercrafts, the 2-DOF dynamics of diving submarines, and 3-DOF autonomous underwater vessels. The method’s use can be easily extended to 6-DOF autonomous underwater submarines and is particularly suitable for cases of underactuation. It can be also used for coordination and synchronization of fleets of USVs and AUVs.