Equation (2.28) shows that, in trajectory tracking applications, the sliding surface $\Sigma$ moves with the reference. One can also observe that the equilibrium point $\dot{e} = 0$, $e = 0$ is contained in the surface $\sigma = 0$. Therefore, the control problem becomes a problem of minimization of the distance $\sigma$. Once the surface is reached, the *sliding motion* drives the system on the surface towards the equilibrium point $\dot{e} = 0$, $e = 0$ giving its name to the method.

The design of the command vector $u(t)$ relatively to the sliding surface is based on the Lyapunov theory. Let us define the following Lyapunov function candidate:

$$V = \frac{1}{2}\sigma^2 \qquad (2.29)$$

for which one obviously has $V(0) = 0$ and $V(\sigma) > 0$ for $\sigma \neq 0$. The first order time derivative of $V$ is:

$$\dot{V} = \dot{\sigma}\sigma \qquad (2.30)$$

and the stability criteria $\dot{V} < 0$ therefore leads to:

$$\dot{\sigma}\sigma < 0 \qquad (2.31)$$

Equation (2.31) is referred to as the *sliding condition*. The sliding condition must be respected to ensure stability. The evolution of the sliding surface is then chosen to meet the sliding condition at all time. One basic solution is:

$$\dot{\sigma} = -\gamma \operatorname{sign}(\sigma) \qquad (2.32)$$

where $\gamma$ is a control gain to be tuned later. Using $\dot{\sigma} = \ddot{e} + \lambda\dot{e}$, the model Equation (2.27) can be combined to the sliding condition (2.32) to give an expression of the control vector $u$:

$$u = g^{-1}(\mathbf{X})[\ddot{x}^* - \lambda\dot{e} - \gamma \operatorname{sign}(\sigma)] - f(\mathbf{X}) \qquad (2.33)$$

Equation (2.33) can also be written as:

$$u = u_1 + u_2 \qquad (2.34a)$$

$$u_1 = g^{-1}(\mathbf{X})[\ddot{x}^* - \lambda\dot{e}] - f(\mathbf{X}) \qquad (2.34b)$$

$$u_2 = -g^{-1}(\mathbf{X})\gamma \operatorname{sign}(\sigma) \qquad (2.34c)$$
where the so-called equivalent term $u_1$ is in fact a feedback linearizing term of reference $\ddot{x}^*$
(see Section 2.2), and $u_2$ is the switching term assuring convergence towards the sliding
surface. In the case of an approximated model, $u_1$ would be based on the model matrices
approximation and would only compensate the known parts of the model matrices.

**Remark 2.1** The conventional sliding surface definition used in this example and in most of the following may lead to the appearance of steady state error in the presence of external unknown disturbances. This phenomena is described in appendix A as well as an alternative sliding surface definition called Integral SMC. The sliding surfaces used in most of the following examples can be replaced with integral sliding mode surfaces allowing better robustness to unknown external disturbances. The conventional sliding surface presented here has a proportional derivative structure which can lead to steady state error and notably when using smooth switching functions. Adding an integral term in the sliding surface definition creates a PID behavior.

Note that, in a real application, the simple controller (2.33) is likely to create chattering
as the switching of the signum function would not be instantaneous. The following exam-
ples propose alternative switching functions reducing or avoiding chattering. Figure 2.3
gives an overview of some of the switching functions used in the following examples to
avoid chattering. This functions are continuous around the equilibrium point which limits
the fast oscillations causing chattering but also modify the convergence of the controllers.
Some work has been done to compare different switching strategies and [134] shows that
SM is globally exponentially stable with this selection of smooth switching functions.

It is also worth mentioning the works of [147, 78, 159, 96] where SMC is combined
with Fuzzy logic to create more robust and adaptive controllers. Adaptive sliding mode
controllers can also be found in the work of [151, 150].

2.4.1 Application to Fully Actuated Vehicles

The work of [175] gives a good example of SMC applied to fully actuated marine
craft, and the formulation is the same as in the example exposed before. The system has
three actuated DOF and is sent on a horizontal plane application composed of (x, y, ψ)
trajectories. In [175], the model is not entirely known a priori and estimated values of
the model matrices are used in the controller. The model also includes approximated
disturbances. All approximations are bounded, and this work shows that, when they are
<figure><img src="image_16.png" /><figcaption>Figure 2.3 – Comparison of the switching functions used to avoid chattering.</figcaption></figure>
known, the estimation bounds can be used for the calculation of the gain parameter $\gamma$ (noted $K(X,t)$ in the reference) to ensure convergence. In addition, Ref. [175] displays the use of a different switching function based on a saturation function instead of a signun function. The saturation function shown in figure 2.3 is better suited for real applications since it avoids the chattering phenomenon. Using the saturation function allows for creating a boundary layer around the sliding surface where the switching effect is made continuous. The size of the boundary layer can be tuned for adapted behavior of the system. The saturation function is originally defined as:

$$ \text{sat}(x) = \begin{cases} \text{sign}(x) & \text{if } |x| \ge 1 \\ x & \text{if } |x| < 1 \end{cases} $$

Using the saturation function, it appears clearly that, inside the boundary layer $|\sigma| < 1$, the nonlinear controller (2.33) becomes very similar to the PID-based linearizing controllers introduced in Section 2.2. Inside of the boundary layer, the controller becomes:

$$ u = g^{-1}(\mathbf{X})[\ddot{x}^* - \lambda\dot{e} - \gamma\sigma] - f(\mathbf{X}) \quad (2.35) $$

Therefore, the main drawback of using a continuous switching function such as the saturation function is that the convergence cannot be guaranteed in finite time anymore. Nonetheless, [175] gives a method for calculation of the control gain, the slope of the surface and the boundary layer thickness based on the estimation of the model matrices, and shows very good tracking results.

The method is then used for the design of three decoupled controllers, one per actuated DOF. For each DOF, a sliding surface is defined as well as the different control parameters such as the boundary layer thickness or the switch control gain. The three sliding surfaces are given as:

$$ \sigma_u = e_u + \lambda_u e_x \quad (2.36a) $$

$$ \sigma_v = e_v + \lambda_v e_y \quad (2.36b) $$

$$ \sigma_r = e_r + \lambda_r e_\psi \quad (2.36c) $$

The surge and sway surfaces given by (2.36a) and (2.36b), respectively, are defined in terms of positions in the inertial frame $\mathbf{R}_0$ and velocities in the body-fixed frame $\mathbf{R}_B$. While this could be fine for simple mono-axial applications, such a definition of
the surge and sway sliding surfaces may be problematic when it comes to more complex trajectories. In this case, the trajectory tracking results show good performances on the tested trajectories.

The work of [34] is another good example of SMC following the same overall logic. Here, the vehicle is controlled only in the diving plane and is fully actuated w.r.t. the task. In this work, the performances of four different controllers based on SMC are compared. The main difference between the four controllers is the model used in the equivalent part $U_1$ (see below Equation (2.39)). The first one uses a linearized model, the second one uses the nonlinear exact model, the third one uses an adaptive model, and the last one uses estimated states. The compliance and robustness of the SMC method make such differences between controllers applied to the same system possible and is once again demonstrated by the results of this work.

The definition of the sliding surface is also slightly different in [34] since a first order linear surface, defined by $\sigma = \gamma^\top x$, is used. Here, $x \in \mathbb{R}^3$ is a reduction of the state of the vehicle in the diving plane, $\gamma \in \mathbb{R}^3$ is a vector of gains, and $\sigma$ contains, in fact, three decoupled surfaces, one per DOF, linearly added. The use of first order sliding surfaces allows for a simpler command vector and enables the use of pole-placement techniques when tuning the parameters.

Another different definition of the set of sliding surfaces is given in [87]. As in [175], the sliding surfaces are defined in terms of the position errors in the inertial frame summed to velocity errors in the moving frame. The set of sliding surfaces is given by:

$$ \sigma(e_\nu, e_\eta) = \begin{bmatrix} \Lambda_1 & \Lambda_2 \end{bmatrix} \begin{bmatrix} e_\nu \\ e_\eta \end{bmatrix} \qquad (2.37) $$

where $\Lambda_1$ and $\Lambda_2$ are coefficient matrices defined in $\mathbb{R}^{6 \times 6}$ and identified in the following. To take better account of the coupling effects between the DOF of the system, the sliding surfaces used here are defined over the full state space while, in the references presented before, they are often defined only on the output. For ease of understanding of the following equations, another self-explanatory expression of the vehicle model is given to match both
the notation of chapter 1 and the notation used in the reference:

$$M\dot{\nu} = f(\nu, \eta) + g(\nu, \eta)u(t) \qquad (2.38a)$$

$$\dot{\eta} = h(\nu, \eta) \qquad (2.38b)$$

The definition of the sliding surfaces (2.37) leads to a different expression of the control vector:

$$U = U_1 + U_2 + U_3 \qquad (2.39a)$$

$$U_1 = \hat{g}(\nu, \eta)^{-1} [(\Lambda_1 M^{-1})^{-1} \dot{\nu}^* - \hat{f}(\nu, \eta)] \qquad (2.39b)$$

$$U_2 = \hat{g}(\nu, \eta)^{-1} (\Lambda_1 M^{-1})^{-1} \Lambda_2 [\dot{\eta}^* - \hat{h}(\nu, \eta)] \qquad (2.39c)$$

$$U_3 = -\hat{g}(\nu, \eta)^{-1} (\Lambda_1 M^{-1})^{-1} F(\sigma, \Phi) \qquad (2.39d)$$

In (2.39), the estimates $\hat{f}$, $\hat{g}$ and $\hat{h}$ of the model functions are used. A similar definition of the control vector $U$ could be given with the real values of the model functions if they were to be considered known. As for the usual definition of SMC given earlier in this section, the control vector $U$ can be broken down into three parts. $U_1$ compensates the estimates of the dynamic effects of the model, $U_2$ provides stabilization based on estimates of the positional elements in $h$, and $U_3$ is the switching term driving the system to the sliding surfaces. This work uses the hyperbolic tangent function over the boundary layer of thickness $\Phi$ as a switching function:

$$F(\sigma, \Phi) = \gamma \tanh(\frac{\sigma}{\Phi}) \qquad (2.40)$$

The hyperbolic tangent function is displayed in figure 2.3 with $\Phi = 0.5$. The hyperbolic tangent appears ideal since it is smooth around the equilibrium point $\sigma = 0$ but is still steep enough around the sliding surface to ensure fast switching and the sliding behavior.

Here, again, the structure of the control law is very similar to some controllers introduced in Section 2.2 and notably when the distance to the sliding surface is close to zero.

As for the saturation function seen before, using the hyperbolic tangent avoids chattering at the cost of a non-finite convergence time.

In order to set the values of $\Lambda_1$ and $\Lambda_2$, the system is linearized. Doing so highlights that the first parameter matrix $\Lambda_1$ can be chosen as the identity matrix without loss of
generality in the fully actuated case. Then, analyzing the linearized close-loop dynamics leads to a desirable choice of the second parameter matrix $\Lambda_2$:

$$ \nu = \Lambda_2 \eta \qquad (2.41) $$

Equation (2.41) is quite similar to the kinematic model of the system given in equation (1.1).

The authors then apply the SMC method they introduced on a set of four Single-Input-Multiple-States subsystems. The three main subsystems are tested first independently then all together in association with LOS guidance in a waypoint tracking experiment. Effects of sea current are also highlighted in the last simulations.

Over the years, some more work has been carried out on SMC and notably the introduction of new sliding surfaces. As an example, terminal sliding mode introduced in [117] and applied to the marine craft model in [113] or [112] is based on a different sliding surface definition. Terminal sliding mode offers finite convergence time, faster and more precisely than conventional sliding mode by introduction of a new nonlinear term in the sliding surface definition. Note that, in this work, the sliding surfaces are completely defined in the inertial frame. In fact, two formulations of the terminal sliding surface are given in [113]:

$$ \sigma_c = \dot{e}(t) + \lambda e(t)^{\frac{q}{p}} = 0 \qquad (2.42a) $$

$$ \sigma_n = e(t) + \frac{1}{\lambda} \dot{e}(t)^{\frac{p}{q}} = 0 \qquad (2.42b) $$

In (2.42), $\lambda$ is analogous to the control parameter used in the previous examples, and $p$ and $q$ are two positive odd integers satisfying $p > q$. Using the sliding condition (2.31), two control vectors can be derived from (2.42), respectively:

$$ \tau_{\eta} = \hat{M}_{\eta} \left( \ddot{\eta}^* + \lambda \frac{q}{p} e(t)^{\frac{q-1}{p}} \dot{e}(t) + \gamma \operatorname{sign}(\sigma_c) \right) + \hat{N}_{\eta}(\nu, \eta, \dot{\eta}) \qquad (2.43a) $$

$$ \tau_{\eta} = \hat{M}_{\eta} \left( \ddot{\eta}^* + \lambda \frac{q}{p} \dot{e}(t)^{2-\frac{p}{q}} + \gamma \operatorname{sign}(\sigma_n) \right) + \hat{N}_{\eta}(\nu, \eta, \dot{\eta}) \qquad (2.43b) $$

The first expression of the terminal sliding surface (2.42a) shows a singularity in $e(t) = 0$ because $\frac{q}{p} < 1$. Therefore, the second expression (2.42b) is to be used. Of course, the two expressions of (2.42) are equivalent when the system reaches the surface. Once again, the
signum function is substituted with a saturation function in the final controllers.

Simulation results show the performances of the terminal sliding mode in comparison with traditional SMC and a classical computed torque controller (CTC). Terminal sliding mode seems to outperform the traditional SMC and CTC notably displaying better convergence times and a smoother overall behavior on the helix tracking task.

Another example of a different formulation of the sliding surface can be found in [4]. This work proposes a controller based on Super Twisting Sliding Mode Control (STSMC) applied to the linearized model of the vehicle in the diving plane. More details about the theory behind STSMC, sliding order and sliding accuracy can be found in [109, 95]. STSMC is designed to have as good disturbance rejection and robustness as traditional SMC but reducing the chattering effect without the need of substituting the sign function, therefore assuring finite time convergence. The Super Twisting behavior is created using a different solution of the sliding condition (2.31):

$$ \dot{\sigma} = \gamma_1 |\sigma|^{\frac{1}{2}} \text{sign}(\sigma) + \gamma_2 \int_0^t \text{sign}(\sigma(\zeta)) d\zeta \quad (2.44) $$

The continuous switching behavior is created by the first member of equation (2.44) displayed on figure 2.3. The second member of the sliding surface definition creates an integral effect on the sliding surface canceling possible steady state errors.

Using the small angles approximation to linearize the system in the diving plane, one can derive the sliding condition given by (2.44) into the following control law:

$$ u = \frac{1}{d_1}(-c_{11}w - c_{12}q - c_{13}\theta - c_{14}z + u_0\dot{\theta} - \lambda\dot{e}_z + \gamma_1\sigma^{\frac{1}{2}}\text{sign}(\sigma) + \gamma_2\int_0^t \text{sign}(\sigma(\zeta))d\zeta) \quad (2.45) $$

In this case, a constant depth $z^*$, $\dot{z}^* = \ddot{z}^* = 0$ is tracked. The $c_{ij}$ coefficients are model parameters and the control input $u$ is analogous to the stern plan deflection angle $\delta_s$. $u_0$ is a constant sway speed.

The simulation results indeed show dampening of the chattering effect when the STSMC is compared with traditional SMC. However, it should be noted that STSMC displays slightly slower convergence than SMC.

This section demonstrated the use of different sliding mode controls in the case of fully actuated marine vehicles. As seen in this section, sliding mode control comes in different
shapes. However, the two main levers for tweaking the controllers are the definition of the sliding surface itself and the choice of the solution of the sliding condition. Many more examples would be worth adding to this section like the work in [179] where SMC is associated with LOS guidance and Fuzzy Logic in the diving control of an autonomous bio-mimetic dolphin robot as well as [149], which introduces four decoupled SM controllers of different orders in a vehicle with actuated surge, heave, pitch and yaw.

## 2.4.2 Application to Underactuated Vehicles

This section studies the use of SMC in the case of underactuated marine craft. It is notably interested in how the SMC method can be used as a guidance principle and allow compensation of underactuation in the same way LOS does and how SMC can be coupled with external guidance laws.

As a first example, in [10], the model of a ur-ship tracking horizontal straight lines is studied. Two decoupled controllers are designed one for the surge motion and the other for the yaw motion. Two sliding surfaces are given:

$$ \sigma_u = e_u + \lambda_u \int_0^t e_u(\zeta) d\zeta \qquad (2.46a) $$

$$ \sigma_r = \dot{e}_v + 2\lambda_r e_v + \lambda_r^2 \int_0^t e_v(\zeta) d\zeta \qquad (2.46b) $$

There are multiple points of interest in (2.46). First, both sliding surfaces are completely defined in the mobile frame. However, because positions in the mobile frame can not be measured, the surfaces are defined in terms of the integral of the velocity error on the two axes of the moving frame. This solution is analogous to defining the surfaces in terms of positions<fnref n="4" /> but, as shown later, the definition of the surge and sway references matters when it comes to tracking trajectories originally defined in the inertial frame. Then, while the surge sliding surface $\sigma_u$ is of the first order relatively to the integral of $e_u$, the yaw sliding surface $\sigma_r$ is of the second order and defined relatively to the sway speed error $e_v$ instead of the heading angle or the yaw velocity. Therefore, the yaw controller derived from $\sigma_r$ will be based on the sway speed error $e_v$. Doing so allows the authors to compensate for the lack of sway actuation with yaw. This method is very close to the other guidance methods exposed earlier in this review, and it is somewhat equivalent

<footnote n="4">4. Note that the definition of the sliding surfaces (2.46) is not considered as Integral SMC as seen in appendix A</footnote>
to generating yaw controls based on the lateral error measured on the yB axis of the moving frame.

The surge and yaw controllers are derived from the respective sliding surfaces and the dynamic model of the system as:

$$ \tau_u = -m_{22}v_r + d_1u - m_{11}(-\dot{u}^* + \lambda_u e_u) - \gamma_u \text{sign}(\sigma_u) \quad (2.47a) $$

$$ \tau_r = \frac{m_{33}}{u^* - \frac{m_{11}}{m_{22}}u} \left[ \frac{r\tau_u}{m_{22}} - v_r - v_{rd} - (2\lambda_r \dot{e}_v + \lambda_r^2 e_v) - \gamma_r \text{sign}(\sigma_r) \right] \quad (2.47b) $$

Note that, in the original article, estimated values of the model parameters are used in the control calculations, and the authors left the possibility to consider nonlinear damping. Equation (2.47) has been simplified for clarity. Equation (2.47b) is of a different structure than the other control equations seen in this section because it is based on the first time derivation of the sway equation of the dynamic model instead of the first order derivative of the chosen sliding condition solution (2.46b). Hence the new speed terms $v_r$ and $v_{rd}$ which contain the terms created by this manipulation. Exact definitions of $v_r$, $v_{rd}$ as well as the gains $\gamma_u$ and $\gamma_r$ can be found in [10]. The dynamic sway equation derivative is used instead of the sliding condition derivative because the latter would require information upon the second order derivative of both the actual and desired sway speeds. Such information is not available in this work.

However, as claimed in [181, 40], the solution proposed in [10] does not solve all the tracking problems. In fact, defining the sliding surfaces in terms of the integrals of the velocity errors in the moving frame leads to neglecting constant offsets of the desired position signals. To counteract this problem, the work of [181] and [40] redefine the surge and sway velocity references.

First [181], gives a similar approach for trajectory tracking of a ur-ship in the horizontal plane but with modified velocity references. To take a possible position offset into account, the desired reference surge and sway speeds are calculated as:

$$ u^* = \cos \psi \dot{x}^* + \sin \psi \dot{y}^* - k \cos \psi e_x - k \sin \psi e_y \quad (2.48a) $$

$$ v^* = -\sin \psi \dot{x}^* + \cos \psi \dot{y}^* + k \sin \psi e_x - k \cos \psi e_y \quad (2.48b) $$

with k a positive constant control parameter. Note that, in the original work [181], two different control parameters $k_1$ and $k_2$, are mentioned but do not appear in the equations. This work shows that convergence of the surge and sway errors $e_u$ and $e_v$ leads to conver-
gence of the position errors in the inertial frame $e_x$ and $e_y$. The first order derivative of the second order yaw sliding surface expressed in terms of the integral of the sway error is used for the calculation of the yaw control. Therefore, knowledge about the second order derivative of the desired sway motion is necessary as well as the third order derivative of both desired position signals.

In the same way, [40] proposes a more global solution than in [10], solving the case of offset trajectories. To do so, new surge and sway speed references are defined as:

$$ \begin{bmatrix} u^* \\ v^* \end{bmatrix} = \begin{bmatrix} \cos \psi & \sin \psi \\ -\sin \psi & \cos \psi \end{bmatrix} \begin{bmatrix} \dot{x}^* + l_x \tanh(-\frac{k_x}{l_x}e_x) \\ \dot{y}^* + l_y \tanh(-\frac{k_y}{l_y}e_y) \end{bmatrix} \quad (2.49) $$

where $k_x$ and $k_y$ are controller gains, and $l_x$ and $l_y$ are saturation coefficients chosen relatively to the system's physics. The definition of the speed references $u^*$ and $v^*$ given by Equation (2.49) leads to the error equation:

$$ \begin{bmatrix} e_u \\ e_v \end{bmatrix} = \begin{bmatrix} \cos \psi & \sin \psi \\ -\sin \psi & \cos \psi \end{bmatrix} \begin{bmatrix} \dot{e}_x + l_x \tanh(-\frac{k_x}{l_x}e_x) \\ \dot{e}_y + l_y \tanh(-\frac{k_y}{l_y}e_y) \end{bmatrix} \quad (2.50) $$

Equation (2.50) shows that convergence of the speed errors $e_u$ and $e_v$ to zero leads to asymptotic convergence of the position errors $e_x$ and $e_y$. In fact, the speed errors in Equation (2.50) behave nearly like sliding surfaces for the positions. The rotation matrix being non-singular, when the speed errors converge to $e_u = e_v = 0$, one obtains:

$$ \dot{e}_x - l_x \tanh(-\frac{k_x}{l_x}e_x) = 0 \quad (2.51a) $$

$$ \dot{e}_y - l_y \tanh(-\frac{k_y}{l_y}e_y) = 0 \quad (2.51b) $$

Equation (2.51) can be seen as the sliding conditions of the inner loop of the system assuring asymptotic convergence of the position errors the hyperbolic tangent function being used instead of the signum function. In a way, this controller is quite similar to the two-staged controllers presented in section 2.2.

The sliding surfaces used for the dynamic controllers are then given. The sliding surfaces used in [40] are very similar to (2.46) but with two different control parameters in
the yaw surface:

$$ \sigma_u = e_u + \lambda_u \int_0^t e_u(\zeta) d\zeta \qquad (2.52a) $$

$$ \sigma_r = \dot{e}_v + \lambda_{r,1}e_v + \lambda_{r,2} \int_0^t e_v(\zeta) d\zeta \qquad (2.52b) $$

Here, too, the sliding surface $\sigma_r$ used for yaw control is built upon the sway error $e_v$. Slightly different dynamics than the usual sliding condition (2.31) are imposed on $\sigma_u$ and $\sigma_r$:

$$ \dot{\sigma}_u = -\gamma_{u,1}\sigma_u - \gamma_{u,2} \operatorname{sign}(\sigma_u) \qquad (2.53a) $$

$$ \dot{\sigma}_v = -\gamma_{v,1}\sigma_v - \gamma_{v,2} \operatorname{sign}(\sigma_v) \qquad (2.53b) $$

where $\gamma_{i,j}$ are all strictly positive control parameters. Note that using this sliding condition adds new terms, equivalent to proportional and derivative terms, in the controllers.

This time, the surge and yaw controllers are both calculated with the first order derivations of the sliding surfaces giving:

$$ \tau_u = -X_u u - a_{23} v r + \frac{1}{M_{11}} (\dot{u}^* - \lambda_u e_u - \gamma_{u,1} \sigma_u - \gamma_{u,2} \operatorname{sign}(\sigma_u)) \qquad (2.54a) $$

$$ \tau_r = -N_r r - a_{12} u v + \frac{1}{b} (-M_2(Y_v \dot{v} + a_{13} \dot{u} r)\Gamma - \lambda_{r,1} \dot{e}_v - \lambda_{r,2} e_v \\ - \gamma_{r,1} \sigma_r - \gamma_{r,2} \operatorname{sign}(\sigma_r)) \qquad (2.54b) $$

The newly introduced mass coefficients $a_{12}$, $a_{23}$, $M_{11}$ and $M_2$ as well as control parameters $\Gamma$ and $b$ are given in detail in [40]. The damping surge, sway and yaw coefficients $X_u$, $Y_v$ and $N_r$, respectively, are issued from the damping matrix of the dynamic model. The simulation results show good performance in tracking linear trajectories with or without an offset and circular trajectories.

A more recent example of SMC applied to underactuated vehicles can be found in [172]. In this work, the authors propose a Super Twisting SMC based solution for the problem of leader-follower tracking. The followers are uqr-vehicles following the trajectory set by the mother ship. Three dimensions LOS guidance is used to calculate the approach angle references in pitch and yaw. Then, three decoupled Super Twisting sliding mode controllers are designed for surge, pitch and yaw as well as a fourth one looping back on
the forward speed of the mother ship.

For surge control of the follower submarines, a zero-order sliding surface is built upon the surge error with a constant reference $u^*$. The surface is therefore given as $\sigma_u = e_u$ and the Super Twisting formulation of the sliding condition is used as in [4]

$$ \dot{\sigma}_u = -\gamma_{u,1}|\sigma_u|^{\frac{1}{2}} \text{sign}(\sigma_u) - \gamma_{u,2} \int_0^t \text{sign}(\sigma_u(\zeta))d\zeta \quad (2.55) $$

with two strictly positive gain parameters $\gamma_{u,1}$ and $\gamma_{u,2}$. The definition of the kinematic Super Twisting sliding-mode controller calculating the surge speed consign of the mother ship is similar to this one. The mother ship velocity is calculated with the position error kinematic model. More details about mother ship control can be found in the reference [172].

On the other hand, the pitch and yaw controllers are Super Twisting sliding mode controllers using first order terminal sliding surfaces as in [113]. The pitch and yaw sliding surfaces and associated sliding conditions are given by

$$ \sigma_{\theta} = e_{\theta} + \lambda_{\theta} |\dot{e}_{\theta}|^{\frac{p_{\theta}}{q_{\theta}}} \quad (2.56a) $$

$$ \dot{\sigma}_{\theta} = -\gamma_{\theta,1}|\sigma_{\theta}|^{\frac{1}{2}} \text{sign}(\sigma_{\theta}) - \gamma_{\theta,2} \int_{0}^{t} \text{sign}(\sigma_{\theta}(\zeta))d\zeta \quad (2.56b) $$

$$ \sigma_{\psi} = e_{\psi} + \lambda_{\psi} |\dot{e}_{\psi}|^{\frac{p_{\psi}}{q_{\psi}}} \quad (2.56c) $$

$$ \dot{\sigma}_{\psi} = -\gamma_{\psi,1}|\sigma_{\psi}|^{\frac{1}{2}} \text{sign}(\sigma_{\psi}) - \gamma_{\psi,2} \int_{0}^{t} \text{sign}(\sigma_{\psi}(\zeta))d\zeta \quad (2.56d) $$

where all the control parameters are constant and strictly positive.

As seen in the previous examples, the actual control signals $\tau_u$, $\tau_q$ and $\tau_r$ are derived from the first order derivative of the associated sliding surface and calculated with the corresponding dynamic model equations. For increased robustness to external disturbances, adaptive disturbance terms can be added to the controller equations as shown in [172]. Finally, the four sliding mode controllers are shown to stabilize the tracking errors of the following vehicles.

These few examples show that SMC can be used in the underactuated case and displays good performances. In these examples, one can find the idea of using a rotational degree of freedom to compensate the lack of one or two non actuated translations. Sliding mode itself can be used as guidance principle like in [10, 40, 181] where yaw controls are given as functions of the lateral motion errors using the same idea as in the LOS examples given
earlier [106] directly in the sliding surface calculations.

## 2.5 Adaptive Control

While this work is essentially focused on model-based control methods, adaptive and model-free controllers are worth mentioning. These methods have received a great amount of interest in the past few years in and outside the scope of marine vehicles [97]. With this type of controllers, the model of the vehicle and the disturbance to which it is subjected to are considered unknown or partially known. These controllers adapt during the application either by changing the gain parameters, estimating the external effects on the vehicle or approximating model parameters. Adaptive controllers are therefore naturally robust to external disturbance and environment changes.

One of the first example of adaptive controllers applied to underwater vehicles can be found in [177] where an adaptive sliding mode controller is experimented on an underwater vehicle. Later, [6] proposes several adaptive controllers to compensate the effects of an unknown sea current using either adaptive gain parameters or a current velocity estimator. Following the same idea, [183] proposes a disturbance observer associated with a self-adjusting controller with adaptive gains. This work also raises that chattering can occur when using self-adjusting control gains and proposes an alternative formulation dampening the chattering effects. The interested reader is also referred to [9, 71, 118, 158, 182], for examples of PID-based controllers using adaptive structures for linearization. Other examples of adaptive sliding-mode controllers can be found for instance in [76].

In [141], two adaptive controllers are presented. The first one is based on a delayed estimate of the model parameters. The second one uses the known bounds of the nonlinear effects of the model to estimate and compensate the worst-case scenario of the disturbing effects. This second controller is shown to be much more efficient than the first one.

Several other methods have been developed over the years to adjust the gains of an adaptive controller or estimate the disturbances such as in [114] which uses fuzzy logic to compute gains or [185] which is based on neural network estimation of the state.

Arguably, integral action is the first step to adaptability in the controllers and is sufficient to compensate most of the effects acting on an AUV [7]. This integral action can take place at the guidance level to create new equilibriums and compensate, for example, sea current as shown in [16, 24]. In the case of adaptive LOS guidance, [61] shows that the integral term can be used in several places to compensate the crab angle
for instance.

One type of adaptive controller is described in more details in the next Section as it used in the following developments, the so-called Intelligent PID (iPID) controller.

## 2.5.1 Intelligent PID

Model-free control based on iPIDs is described in details in [49] which unifies the previous works done on the subject in the references therein. It is a wide spread model-free control method (more than 400 citations to date) applied to a large range of systems from shape memory alloy actuators [79] to flying drones [33, 11], AUVs [15], autonomous land vehicles [123] and some more industry and research applications.

The idea behind iPIDs is to approximate the system with a very simple linear *ultra-local* model where all the unknown effects are summed up in a single estimator function. It is therefore easily adapted to any system, the ultra-local model and estimation technique are almost always the same (the order of the ultra-local model may change) [50]. The disturbing term of the ultra-local model is estimated with an *ALIEN* filter. Some of the early work done on ALIEN filter can be found for instane in [120] where they are used for estimation of noisy signals.

### 2.5.1.1 Ultra-local model

The perhaps complex model of the system is estimated using the following ultra-local model:

$$y^{(\nu)} = \alpha u + F \qquad (2.57)$$

where $y$ is the system output, $u$ is the input, $\nu$ is the order of the ultra-local model (may differ from the order of the system itself), $\alpha$ is a non-physical constant tuning parameter and $F$ is the continuously updated function estimating all the unknown effects (nonlinear model effects, disturbances).

In most cases, a first order ultra-local model $\nu = 1$ gives good results but sometimes a second order model is preferable. Note that several solutions can exist to approximate a system with an ultra-local model. As an example, consider the second order model representing the dynamic of the position of a vehicle. It is possible to use a second order ultra-local model using the position of the vehicle as a measurement but it is also possible to use a first order ultra-local model using the speed of the vehicle as the measurement. Finally, it could also be possible to use a first order ultra-local model with
position measurement and consider all second order effects as disturbance.

It is straightforward to build a controller based on the ultra-local model (2.57). In this example, a simple proportional control law with a feedforward term is used but more advanced iPID controllers can be useful.

The controller based on the first order ultra-local model is:

$$u = \frac{1}{\alpha} (\dot{y}^* - \hat{F} + K_p e) \qquad (2.58)$$

where $\dot{y}^*$ is the first order derivative of the desired output $y^*$, $e = y^* - y$ is the error between the desired and current output and $K_p$ is a gain parameter. $\hat{F}$ is a short term estimate of $F$.

The closed-loop temporal is then:

$$\dot{e} + K_p e = \hat{F} - F \qquad (2.59)$$

Stability of the closed-loop system (2.59) is hardly demonstrated because of the behavior of the estimate $\hat{F}$ but the large number of successful applications of the method tends to serve as a proof.

The work of [48] shows that iPs and iPIs do not work for second order ultra-local models. In these cases, a derivative term is needed. Also, the work of [127] shows that the estimation of $F$ in iPs and iPDs has an integral effect and makes iPs (respectively iPDs) equivalent to conventional PIs (respectively PIDs). Consequently, iPDs are favored in the following.

### 2.5.1.2 Estimation of $F$

Looking at equation (2.58), it appears clearly that the main difficulty of iPIDs is the estimation of $F$. The estimation of $F$ is performed using ALIEN filters introduced in [58]. The adaptive term $F$ regroups all the unmodeled effects whether they are nonlinear effects of the system or external disturbance with no distinction. The calculations presented in this section are for a first order ultra-local model. Same methodology can be applied to higher orders.

The first hypothesis to estimating $\hat{F}$ is that it is piece-wise constant on a small time window $[t_{k-1}, t_k]$. The ultra-local model at the first order should therefore be expressed as:

$$\dot{y}(t) = \alpha u(t) + F_k, t \in [t_{k-1}, t_k] \qquad (2.60)$$
In the following, the subscript k will be omitted for simplicity and the calculation are performed on the time window $[0, T]$ with $0 < T \ll 1$.

Equation (2.60) gives:

$$F = \dot{y} - \alpha u \qquad (2.61)$$

Because $F$ is considered constant on the time window, equation (2.61) is given in operational form:

$$\frac{F}{s} = sy(s) - y(0) - \alpha u(s) \qquad (2.62)$$

The initial condition $y(0)$ is eliminated differentiating once with respect to $s$:

$$-\frac{F}{s^2} = y(s) + s \frac{dy}{ds}(s) - \alpha \frac{du}{ds}(s) \qquad (2.63)$$

To add a filtering effect, equation (2.63) is multiplied by $1/s^2$ so that every member is integrated at least once when going back to temporal space. Doing so adds robustness to the estimation. Equation (2.63) becomes:

$$-\frac{F}{s^4} = \frac{1}{s^2}y(s) + \frac{1}{s}\frac{dy}{ds}(s) - \alpha \frac{1}{s^2}\frac{du}{ds}(s) \qquad (2.64)$$

Back to temporal, equation (2.64) becomes:

$$-\frac{T^3}{3!}F = \int_0^T \left( \frac{(T-\tau)^1}{1!} - \tau \right) y(\tau) d\tau - \alpha \int_0^T \frac{(T-\tau)^1}{1!} (-\tau) u(\tau) d\tau \qquad (2.65)$$

which simplifies as:

$$F = -\frac{6}{T^3} \int_0^T [(T - 2\tau)y(\tau) + \alpha(T - \tau)\tau u(\tau)] d\tau \qquad (2.66)$$

The estimate can then be shifted into the time window $[t-T, t]$:

$$F = -\frac{6}{T^3} \int_0^T [(T - 2\tau)y(\tau + t - T) + \alpha(T - \tau)\tau u(\tau + t - T)] d\tau \qquad (2.67)$$

The estimate of $F$ (2.67) is adjusted at each sample and used in the controller to compensate for all the nonlinear effects and disturbances. No strict rules are given for the tuning of parameter $\alpha$ except that it should be chosen so that the input and output of the system are of the same order of magnitude. The works of [127] and [133] give guidelines to the tuning of the control parameter based on trial-error.
## 2.6 Conclusions on the literature review

This section shows some of the tremendous work done on the control of autonomous marine craft. A first clear conclusion is that there are many different methods in the literature answering the problem of controlling a marine vehicle with seemingly equivalent performances. It is then a matter of adapting the choice of the control method to the application, vehicle, sensors, etc... The four main solutions studied in this section — Line Of Sight guidance, PID-based control and linearization, Differential flatness and Sliding Mode Control — all show very promising results. These methods are used and detailed further on in the following. The choice of the control method may also be related to the type of scenario at hand in the task. Notably, LOS guidance is mainly used in setpoint regulation and path following or path tracking applications while differential flatness is more suited for trajectory tracking scenarios.

Also, this section shows that the control of underactuated vehicle requires additional mechanisms to quit the traditional one-to-one input-output relationship of conventional controllers and create necessary compensation behaviors. Often times, the idea with underactuated vehicle is to use one of the actuated DOF to compensate the lack of another non-actuated DOF. The classical example of this strategy is the traditional *ur*-vessel where yaw is used to cope for the lack of sway. This strategy can be extended in 3D space and with other associations of actuated rotations and non-actuated translations. The two novel control results presented in chapter 3 and chapter 4 follow this same logic.
# A NEW KINEMATIC GUIDANCE PRINCIPLE
## BASED ON THE $\mathcal{H}$ MATRIX

## Contents

<table><tr><td>3.1 Theoretical Example</td><td>91</td></tr><tr><td>3.1.1 Intuitive controller in the fully actuated case</td><td>92</td></tr><tr><td>3.1.2 Kinematic guidance in the underactuated case</td><td>93</td></tr><tr><td>3.2 Kinematic application</td><td>97</td></tr><tr><td>3.2.1 Interest of a tracking point</td><td>97</td></tr><tr><td>3.2.2 Unitary cases</td><td>98</td></tr><tr><td>3.2.3 Algorithm for the calculation of $\mathcal{H}$</td><td>106</td></tr><tr><td>3.3 Application RSM</td><td>108</td></tr><tr><td>3.3.1 Actuation and task of the RSM Robot</td><td>109</td></tr><tr><td>3.3.2 Complete controller</td><td>113</td></tr><tr><td>3.3.3 Stability proof</td><td>115</td></tr><tr><td>3.4 Simulation results</td><td>121</td></tr><tr><td>3.4.1 Comparison of compensation solutions</td><td>121</td></tr><tr><td>3.4.2 Comparison with LOS and SMC</td><td>126</td></tr><tr><td>3.5 Path Following</td><td>132</td></tr><tr><td>3.5.1 The Remus100 AUV</td><td>132</td></tr><tr><td>3.5.2 $\mathcal{H}$ matrix controller for Remus100</td><td>133</td></tr><tr><td>3.5.3 The path-following problem</td><td>135</td></tr><tr><td>3.5.4 Simulation results</td><td>140</td></tr><tr><td>3.6 Conclusion</td><td>147</td></tr></table>

This chapter introduces a new model-based kinematic guidance principle for underactuated marine vehicles based on the new *Handy* matrix $\mathcal{H}$. This controller is notably
useful for trajectory tracking applications of ill-actuated AUVs. It relies on the idea of compensating the lack of actuation of one translation with an actuated rotation. This idea appears indirectly in many of the controllers presented for underactuated vehicles in chapter 2. The *H* matrix acts as a non-diagonal gain matrix based on model coefficients. The compensation mechanism created by the matrix *H* is in fact a kinematic guidance principle included in the control law. The same principle appears for instance in the SMC for underactuated AUVs presented in section 2.4.2.

Outside of the scope of marine vehicles, other model-based methods are used for compensation of ill-actuation of mechanical systems. As an example, the *partial feedback linearization* introduced in [161, 160] relies on dynamic coupling effects between active and passive DOF. Under some restrictions, this work shows that passive DOF can be controlled exploiting their relations with active ones. Note that some marine crafts meet the *strong inertial coupling* condition introduced in [161] and could potentially be controlled using the *non-collocated* controller.

The work of Spong on *collocated* and *non-collocated* control as well as the multiple articles of Fossen on LOS guidance are behind the idea of the control method introduced in this chapter. The *H* matrix and the associated controller have been designed to allow exploiting the natural kinematic couplings of the vehicle to control some of the passive DOF using the actuated DOF.

**3.1 A theoretical example of model based compensa-**
**tion mechanisms**

In this section, as a theoretical example, a linear system is studied to demonstrate the
methodology used in the design of the *H* matrix controller. First, an intuitive model-based
controller is designed for the fully-actuated system. This controller is based on the inverse
model of the system and a Proportional-Integral control law. It gives straightforward
independent convergence of every state components of the system towards the desired
value, naturally canceling any coupling terms of the system. Then, an underactuated
version of the system is introduced and the first controller is shown to be insufficient
for proper control of the underactuated system. Finally, the model-based compensation
mechanism is introduced in the controller by means of a new model-based asymmetrical
term and is shown to allow convergence of the system towards the trajectory.
### 3.1.1 Intuitive controller in the fully actuated case

Before introducing the $\mathcal{H}$ matrix controller in the context of an underwater vehicle, the method is described on a simple, theoretical example. As it will be demonstrated later on, this example mimics the kinematic model of an AUV restricted to the horizontal plane. Let us introduce the following fully-actuated system:

$$ \dot{x}_1 = u_1 \qquad (3.1a) $$

$$ \dot{x}_2 = u_2 + \varepsilon u_3 \qquad (3.1b) $$

$$ \dot{x}_3 = u_3 \qquad (3.1c) $$

where the input vector is $\mathbf{u} = [u_1 \ u_2 \ u_3]^\top$ and the state is $\mathbf{x} = [x_1 \ x_2 \ x_3]^\top$. The input-output relationship of system (3.1) can be rewritten as $\dot{\mathbf{x}} = \mathbf{T}\mathbf{u}$ with:

$$ \mathbf{T} = \begin{bmatrix} 1 & 0 & 0 \\ 0 & 1 & \varepsilon \\ 0 & 0 & 1 \end{bmatrix} \qquad (3.2) $$

In this example, the system is fully actuated and the off-diagonal term of $\mathbf{T}$ creates a coupling effect between the third input component $u_3$ and the second DOF of the state $x_2$ equation (3.1b). As demonstrated later, this example is equivalent to controlling the velocity of a given point on a solid body $\dot{\mathbf{x}}$ ($\dot{x}_1$ and $\dot{x}_2$ being linear velocities and $\dot{x}_3$ being an angular speed) with the velocity of another point $\mathbf{u}$ ($u_1$ and $u_2$ are linear velocities, $u_3$ is an angular speed) through the transformation matrix $\mathbf{T}$.

We now build a model-based controller to drive the state of the system $\mathbf{x}$ towards a desired state $\mathbf{x}^*$. In this first example, the system is fully actuated and can therefore track all three components of the desired state. The following intuitive controller can be
designed:

$$ \mathbf{u} = \mathbf{T}^{-1}(\dot{\mathbf{x}}^* + \boldsymbol{\lambda}(\mathbf{e}_x)) \qquad (3.3a) $$

$$ \mathbf{e}_x = \mathbf{x}^* - \mathbf{x} \qquad (3.3b) $$

$$ \boldsymbol{\lambda}(\mathbf{e}_x) = \mathbf{K}_p \mathbf{e}_x + \mathbf{K}_i \int_0^t \mathbf{e}_x(\zeta) d\zeta \qquad (3.3c) $$

$$ \mathbf{T}^{-1} = \begin{bmatrix} 1 & 0 & 0 \\ 0 & 1 & -\epsilon \\ 0 & 0 & 1 \end{bmatrix} \qquad (3.3d) $$

The control input vector $\mathbf{u}$ is built using the inverse of model (3.1) where a feedforward term $\dot{\mathbf{x}}^*$ associated with a compensator function $\boldsymbol{\lambda}(\mathbf{e}_x)$ have been plugged in place of the derivative of the state $\dot{\mathbf{x}}$. In this example, $\boldsymbol{\lambda}$ contains three decoupled PI controllers based on the tracking error $\mathbf{e}_x$ with $\mathbf{K}_p$ and $\mathbf{K}_i$ two appropriate diagonal matrices of control gains. Other usual controllers could be used instead.

This kind of structure for a controller is very well known and notably in the autonomous marine vehicles community (See section 2.2.2 or [63] for examples). With this controller, the closed-loop system is:

$$ \dot{\mathbf{x}} = \mathbf{T}\mathbf{T}^{-1}(\dot{\mathbf{x}}^* + \boldsymbol{\lambda}(\mathbf{e}_x)) = \dot{\mathbf{x}}^* + \boldsymbol{\lambda}(\mathbf{e}_x) \qquad (3.4a) $$

$$ \dot{\mathbf{e}}_x + \mathbf{K}_p \mathbf{e}_x + \mathbf{K}_i \int_0^t \mathbf{e}_x(\zeta) d\zeta = 0 \qquad (3.4b) $$

which, for appropriate choices of the gain parameters (diagonal, positive definite), ensures global exponential convergence of the three components of the state towards the desired values.

Note that the model-based structure of the control vector $\mathbf{u}$ naturally cancels the coupling effects of the non-diagonal matrix $\mathbf{T}$ and allows independent convergence of the two DOF $x_2$ and $x_3$.

### 3.1.2 Kinematic guidance in the underactuated case

Consider now that the second input component $u_2$ of the previous system (3.1) is not available anymore, the control vector is now of form $\mathbf{u} = [u_1 \ 0 \ u_3]^\top$. The system is now considered underactuated with respect to the task $\mathbf{x}^* = [x_1^* \ x_2^* \ x_3^*]^\top$. The task is
then reduced to the first two components $x_1^*$ and $x_2^*$ to match the new capabilities of the system. Yet, because the actuated DOF of the system do not exactly match the required DOF of the task, it is ill-actuated with respect to the reduced task and a compensating behavior will be mandatory.

Very often, when working with an underactuated system, the non-actuated DOF are neglected and the controller is built on a reduced version of the state space [3, 31]. In this example, the reduced version of equation (3.3) is:

$$ \begin{bmatrix} u_1 \\ u_3 \end{bmatrix} = \begin{bmatrix} 1 & 0 \\ 0 & 1 \end{bmatrix} \begin{bmatrix} \dot{x}_1^* + \lambda_1(e_{x_1}) \\ \dot{x}_3^* + \lambda_3(e_{x_3}) \end{bmatrix} \qquad (3.5) $$

where the second row of the problem corresponding to the non-actuated DOF has been removed. In order to keep a square relationship, the second column of $T^{-1}$ has also been discarded in (3.5), hence neglecting the coupling effect created by the off-diagonal term in the second row. The control functions $\lambda_1$ and $\lambda_3$ are marked differently to show that the set of parameters of each of the PI controller can be different.

The reduced controller (3.5) would perfectly work for any application where the first and third DOF $x_1$ and $x_3$ are controlled but, in this case, where $x_2$ is supposed to be controlled, this reduced controller is not applicable. Using this reduced controller on the system (3.1) gives the following closed-loop system:

$$ \begin{bmatrix} \dot{x}_1 \\ \dot{x}_2 \\ \dot{x}_3 \end{bmatrix} = \begin{bmatrix} 1 & 0 & 0 \\ 0 & 1 & \varepsilon \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} u_1 \\ 0 \\ u_3 \end{bmatrix} = \begin{bmatrix} u_1 \\ \varepsilon u_3 \\ u_3 \end{bmatrix} = \begin{bmatrix} \dot{x}_1^* + \lambda_1(e_{x_1}) \\ \varepsilon(\dot{x}_3^* + \lambda_3(e_{x_3})) \\ \dot{x}_3^* + \lambda_3(e_{x_3}) \end{bmatrix} \qquad (3.6) $$

Equation (3.6) indeed shows that the first and third DOF are well controlled towards the desired values $x_1^*$ and $x_3^*$ but the second DOF is not. As expected after space reduction, the second component of the desired state $x_2^*$ does not even appear in the closed-loop system (3.6). However, the third input component $u_3$ appears to act on the second DOF $x_2$ though the coupling term $\varepsilon$. This relationship means that the third control input $u_3$ could be used to control either $x_2$ or $x_3$.

The idea of a model-based compensating controller is then to use this natural coupling effect of the model to generate a control input $u_3$ allowing to track the desired value of
$x_2$. To do so, a new non-diagonal term has to be added to the expression of the controller, the so-called *Handy Matrix* $\mathcal{H}$. The new controller $\bar{u} = [\bar{u}_1 \ 0 \ \bar{u}_3]^\top$ is defined as:

$$ \bar{u} = \mathcal{H}T^{-1}(\dot{x}^* + \lambda(e_x)) \qquad (3.7) $$

The Handy matrix is designed to create a new expression of $\bar{u}_3$ which, when the controller is applied to the system, allows tracking of $x_2$. Therefore, the new control input $\bar{u}_3$ will be a function of the desired state $x_2^*$ and of the correction term term $\lambda_2(e_{x_2})$. Input $\bar{u}_3$ must also cancel any dependence on $x_3$ or $x_3^*$ in the closed-loop equation of $x_2$. The new controller will be calculated on the complete space and not on a reduced space.

In this first example, matrix $\mathcal{H}$ is built iteratively to demonstrate the process but an algorithm computing $\mathcal{H}$ for any applicable system is provided in the following sections. Detailing equation (3.7) gives:

$$ \begin{bmatrix} \bar{u}_1 \\ 0 \\ \bar{u}_3 \end{bmatrix} = \mathcal{H} \begin{bmatrix} 1 & 0 & 0 \\ 0 & 1 & -\epsilon \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} \dot{x}_1^* + \lambda(e_{x_1}) \\ \dot{x}_2^* + \lambda_2(e_{x_2}) \\ \dot{x}_3^* + \lambda_3(e_{x_3}) \end{bmatrix} = \mathcal{H} \begin{bmatrix} \dot{x}_1^* + \lambda_1(e_{x_1}) \\ \dot{x}_2^* + \lambda_2(e_{x_2}) - \epsilon(\dot{x}_3^* + \lambda_3(e_{x_3})) \\ \dot{x}_3^* + \lambda_3(e_{x_3}) \end{bmatrix} \quad (3.8) $$

On the other hand, applying the control vector $\bar{u}$ to the system (3.1) gives:

$$ \begin{bmatrix} \dot{x}_1 \\ \dot{x}_2 \\ \dot{x}_3 \end{bmatrix} = \begin{bmatrix} \bar{u}_1 \\ \epsilon \bar{u}_3 \\ \bar{u}_3 \end{bmatrix} \qquad (3.9) $$

The control inputs $\bar{u}_1$ and $\bar{u}_3$ must create the following expected closed-loop expressions for $\dot{x}_1$ and $\dot{x}_2$:

$$ \dot{x}_1 = \dot{x}_1^* + \lambda_1(e_{x_1}) \qquad (3.10a) $$

$$ \dot{x}_2 = \dot{x}_2^* + \lambda_2(e_{x_2}) \qquad (3.10b) $$

Considering Equations (3.8), (3.9) and (3.10), the coefficients of $\mathcal{H}$ can be reverse-engineered; The relation $\bar{u}_1 = \dot{x}_1^* + \lambda_1(e_{x_1})$ gives the first line of $\mathcal{H}: \begin{bmatrix} 1 & 0 & 0 \end{bmatrix}$. The zero
on the second line of $\bar{u}$ gives the second line of $\mathcal{H}: \begin{bmatrix} 0 & 0 & 0 \end{bmatrix}$.

For the third line, the previous discussion hints that a first term will be needed in the second column to turn $\bar{u}_3$ into a function of $\dot{x}_2^* + \lambda_2(e_{x_2})$. Another term in the third column will also be necessary to cancel the dependence in $x_3^*$. The third line of $\mathcal{H}$ will then be of shape $\begin{bmatrix} 0 & a & b \end{bmatrix}$ and coefficients $a$ and $b$ are found using Equations (3.9) and (3.10):

$$ \bar{u}_3 = \frac{1}{\varepsilon}(\dot{x}_2^* + \lambda_2(e_{x_2})) = \begin{bmatrix} 0 & a & b \end{bmatrix} \begin{bmatrix} \dot{x}_1^* + \lambda_1(e_{x_1}) \\ \dot{x}_2^* + \lambda_2(e_{x_2}) - (\dot{x}_3^* + \lambda_3(e_{x_3})) \\ \dot{x}_3^* + \lambda_3(e_{x_3}) \end{bmatrix} \quad (3.11) $$

The solution of this equation is given by $a = \frac{1}{\varepsilon}$ and $b = 1$. The handy matrix $\mathcal{H}$ for this example is therefore:

$$ \mathcal{H} = \begin{bmatrix} 1 & 0 & 0 \\ 0 & 0 & 0 \\ 0 & \frac{1}{\varepsilon} & 1 \end{bmatrix} \quad (3.12) $$

and the controller reads:

$$ \begin{bmatrix} \bar{u}_1 \\ 0 \\ \bar{u}_3 \end{bmatrix} = \mathcal{H} \begin{bmatrix} \dot{x}_1^* + \lambda_1(e_{x_1}) \\ \dot{x}_2^* + \lambda_2(e_{x_2}) - \varepsilon(\dot{x}_3^* + \lambda_3(e_{x_3})) \\ \dot{x}_3^* + \lambda_3(e_{x_3}) \end{bmatrix} = \begin{bmatrix} \dot{x}_1^* + \lambda_1(e_{x_1}) \\ 0 \\ \frac{1}{\varepsilon}(\dot{x}_2^* + \lambda_2(e_{x_2})) \end{bmatrix} \quad (3.13) $$

Matrix $\mathcal{H}$ is non-diagonal and presents a form of symmetry with the model matrix $\mathbf{T}$. The closed-loop system for this example is then:

$$ \begin{bmatrix} \dot{x}_1 \\ \dot{x}_2 \\ \dot{x}_3 \end{bmatrix} = \begin{bmatrix} 1 & 0 & 0 \\ 0 & 1 & \varepsilon \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} 1 & 0 & 0 \\ 0 & 0 & 0 \\ 0 & \frac{1}{\varepsilon} & 1 \end{bmatrix} \begin{bmatrix} \dot{x}_1^* + \lambda_1(e_{x_1}) \\ \dot{x}_2^* + \lambda_2(e_{x_2}) \\ \dot{x}_3^* + \lambda_3(e_{x_3}) \end{bmatrix} = \begin{bmatrix} \dot{x}_1^* + \lambda_1(e_{x_1}) \\ \dot{x}_2^* + \lambda_2(e_{x_2}) \\ \frac{1}{\varepsilon}(\dot{x}_2^* + \lambda_2(e_{x_2})) \end{bmatrix} \quad (3.14) $$

Equation (3.14) shows that the new control vector $\bar{u}$ of (3.7) allows tracking of the two
first DOF $x_1$ and $x_2$. The third DOF $x_3$ is disturbed by the coupling effect. The disturbance on $x_3$ must be assessed to ensure that the controller will not generate problematic configuration. Ideally, $x_3$ should be naturally stable for better use of this controller.

In this example, the application of the non-diagonal matrix $\mathcal{H}$ to the system exploits the natural coupling of the system to create the expected compensation behavior and allows to control the non-actuated second DOF $x_2$ using the third input $\bar{u}_3$.

## 3.2 Application to the kinematic model of the underwater vehicle

The model-based compensation strategy introduced in the previous section is applied, in this section, to the kinematic model of an underactuated underwater vehicle in the form of the *Handy matrix* $\mathcal{H}$. This section shows that the kinematic model of an underwater vehicle with an off-centered tracking point is very similar to the example system of section 3.1 and can therefore be treated in the same way. Several unitary cases of compensation are given and the algorithm for calculation of the Handy matrix in the general case is detailed and applied to the example of a standard *uqr*-craft. This compensation mechanism will then be used as a kinematic model-based guidance principle on underactuated underwater craft.

### 3.2.1 Interest of a tracking point

In order to reproduce a structure similar to (3.1) in the model of the underwater craft, this method uses an off-centered tracking point as defined in section 1.1.2.1. Tracking points or *virtual reference points* (VRP) appear in the literature [14, 3] as a way to add angular stability to the vehicle. The VRP is usually positioned at the bow of the vehicle and controlling the VRP is equivalent to pulling the vehicle from this point. It is more naturally stable than pushing from the rear or the center, similar to how pulling a trolley is steadier than pushing it from a single rear point [14].

Another noteworthy aspect of using a tracking point is to recreate a similar structure as equation (3.1) in the kinematic model of the vehicle. Indeed, equation (1.3) shows similarities with the example system (3.1) introduced in section 3.1 with the addition of the transformation matrix $\mathbf{J}(\eta)$. The input of the subsystem (1.3) is $\boldsymbol{v}$ and the output is
$\eta_E$. The modified kinematic model is recalled here:

$$ \dot{\eta}_E = J(\eta)T\nu \qquad (3.15) $$

A controller relying on the model-based compensation mechanism introduced in section 3.1 can then be designed for an underactuated underwater vehicle with a tracking point $E$. Following the theoretical example detailed above, the compensating controller applied to the kinematic model of the vehicle will be of shape:

$$ \nu = \mathcal{H}T^{-1}J(\eta)^{-1}\dot{\eta}_E \qquad (3.16) $$

where $\dot{\eta}_E$ denotes a corrective velocity vector expressed in the inertial frame at the tracking point. This vector is a function of the desired velocity vector and of the error between the actual and desired state of the tracking point. It is typically defined as:

$$ \dot{\eta}_E = \dot{\eta}^* + \lambda(e_\eta) \qquad (3.17) $$

where $e_\eta = \eta^* - \eta_E$ is the error vector between the desired state $\eta^*$ and the state of the tracking point $\eta_E$ and $\lambda$ is a vector of decoupled PI controllers, one per DOF.

### 3.2.2 Unitary cases

This Section introduces the unitary cases of compensation that can be combined to design a complete controller. The matrix $T$ of (1.4) shows that multiple coupling effects can appear on the same DOF. Depending on the position of the tracking point $E$ and the propulsive arrangement of the system, different compensation mechanisms can be created.

As a rule of thumb, if $E$ has a non-zero coordinate on one axis of $R_B$, say the $x_B$ axis, then the rotation around one of the two other axis, for instance $z_B$, can be used to generate a translation of point $E$ on the third axis, $y_B$ in this case.

Two binary vectors are introduced to represent the actuation of the system: $h_{O_B}$ and $h_E$. They respectively represent the actuated DOF of the vehicle and the DOF controlled at the tracking point $E$ in the mobile frame. Both vectors have one column and six rows, one per DOF, where “1” means actuated or controlled and “0” means non-actuated or not controlled. As an example, a torpedo-shaped uqr-vessel (See section 5.1 for an example of such a vehicle, Remus100) with a tracking point positioned at the front and following a 3-D $(x, y, z)$ trajectory with no attitude constraint can be represented by the following
pair of vectors:

$$h_{O_B} = \begin{bmatrix} 1 \\ 0 \\ 0 \\ 0 \\ 1 \\ 1 \end{bmatrix} \quad \text{and} \quad h_{E} = \begin{bmatrix} 1 \\ 1 \\ 1 \\ 0 \\ 0 \\ 0 \end{bmatrix} \qquad (3.18)$$

The vector $h_{O_B}$ encodes that the surge, pitch and yaw of the vehicle are actuated at the center of the vehicle $O_B$ and the vector $h_E$ encodes that the surge, sway and heave of the tracking point $E$ are controlled. This pair of vectors implies that pitch and yaw, both in association with surge, have been chosen to compensate for the lack of sway and heave actuation and therefore allows the vehicle to fulfill tasks consisting in three translations and no rotations.

The $h_{O_B}$ vector of a system is imposed by the propulsive configuration whereas the $h_E$ vector is chosen depending on the position of the tracking point and the task. Associating the controlled DOF of the tracking point $h_E$ and the task can be tricky because of the nonlinear relationship between the translations coded in $h_E$ and defined in $R_B$ and the necessary translations of the task defined in $R_0$. If the task requires two translations with no additional angular constraints then any pair of controlled translations of the tracking point can be used to fulfill the task providing that the vehicle is capable of maintaining the right attitude. As an example, if a task is composed of an horizontal trajectory with no angular constraint, the vehicle must be able to move along the $x_o$ and $y_o$ axes. Two orthogonal translations are needed. The obvious choice of controlled DOF for this task is surge and sway (translations on the $x_B$ and $y_B$ axis). But, if the vehicle is also capable of maintaining a constant 90° roll angle, surge and heave could be used instead. In the same way, if the vehicle can maintain a constant 90° pitch angle, sway and heave would also allow fulfilling the task. Therefore, the choice of $h_E$ must take the rotation capabilities of the vehicle into account.

In order to place the tracking point for a given task and vehicle or to choose the propulsive arrangement of a vehicle for a given task and tracking point, table 3.1 gives a list of elementary compensation possibilities.
<table><caption>Table 3.1 – Unitary cases of compensation according to the position of the tracking point $\mathbf{E}$</caption><thead><tr><th><b><i>E</i></b></th><th><b><i>h</i><sub>O<sub>B</sub></sub></b></th><th><b><i>h</i><sub>E</sub></b></th></tr></thead><tbody><tr><td rowspan="2">[ε<sub>x</sub> 0 0]<sup>T</sup></td><td>[1 0 0 0 0 1]<sup>T</sup></td><td>[1 1 0 0 0 0]<sup>T</sup></td></tr><tr><td>[1 0 0 0 1 0]<sup>T</sup></td><td>[1 0 1 0 0 0]<sup>T</sup></td></tr><tr><td rowspan="2">[0 ε<sub>y</sub> 0]<sup>T</sup></td><td>[0 1 0 1 0 0]<sup>T</sup></td><td>[0 1 1 0 0 0]<sup>T</sup></td></tr><tr><td>[0 1 0 0 0 1]<sup>T</sup></td><td>[1 1 0 0 0 0]<sup>T</sup></td></tr><tr><td rowspan="2">[0 0 ε<sub>z</sub>]<sup>T</sup></td><td>[0 0 1 1 0 0]<sup>T</sup></td><td>[0 1 1 0 0 0]<sup>T</sup></td></tr><tr><td>[0 0 1 0 1 0]<sup>T</sup></td><td>[1 0 1 0 0 0]<sup>T</sup></td></tr></tbody></table>

table 3.1 shows the unitary cases of compensation of a non-actuated translation with an actuated rotation according to the position of the tracking point $\mathbf{E}$. It shows which DOF of the tracking point can be controlled depending on its position and the propulsive arrangement of the vehicle.

Several design rules can be drawn from this table:

1. This method can only be used if the tracking point $\mathbf{E}$ is different from $O_B$ the center of $R_B$.

2. This method only allows compensation of translations with rotations.

3. This method cannot be used to compensate a translation on one axis with a rotation around the same axis.

Of course, the unitary cases of table 3.1 can be combined and associated with additional actuated DOF to create a complete propulsive configuration. Note that this set of rules can also be used to understand what kind of task a given AUV is able to complete.

Additional remarks can be made about table 3.1. First, note that, in each case, at least two DOF appear in both $h_{O_B}$ and $h_E$, one being an actuated translation. As an example, with the tracking point $\mathbf{E}$ of coordinates $[\epsilon_x \ 0 \ 0]^T$, yaw can be used to compensate for sway or pitch can be used to compensate for heave but, in both cases, actuation in surge is mandatory. From a kinematic point of view, this additional actuated speed is not strictly necessary to perform compensation of a non actuated linear speed. But, from a control perspective, this DOF is mandatory. It covers the cases where a 90 degrees rotation of the vehicle is not enough to cancel the error on the compensated linear DOF. In such a
scenario, the error is transferred to this other, actuated translation and can be canceled.

Note also that each $h_E$ vector appears twice in table 3.1. Therefore, two equivalent control solutions can be found for two different propulsive configurations and the same set of controlled DOF. As an example, a vehicle actuated in sway and roll with a tracking point on the $y_B$ axis and a vehicle actuated in heave and roll with a tracking point on the $z_B$ axis can both be chosen for a task requiring sway and heave.

As exposed later in this work, using the $H$-based control method can give several solutions for following a task with a given vehicle. It will then be the user's decision to pick among these solutions the more suitable one. One criterion is the need of controlled rotations. Indeed, when a rotation is used for compensation it cannot be controlled any- more. As an example, a *ur*-surface vessel cannot use this method to control both sway and yaw at the same time. However, the uncontrolled rotation is, most of the times, me- chanically stable or stabilized by hydrodynamic effects on the vehicle. In the example of the *ur*-vessel, yaw naturally tends towards the tangent to the track when sway error is canceled and does not need to be actively controlled.

To demonstrate the different possibilities given by this method, simulations of some of the unitary cases of table 3.1 have been conducted. These simulations aim to show that the $H$-based controller can create different vehicle behaviors. It is of course a very simple elementary example and not a realistic task. The trajectory of this example is a vertical rail traveled at constant speed with no angular constraint. Considering that the vehicle arrives at the top of the rail in a neutral horizontal state (no roll or pitch but any yaw), heave control of the tracking point is mandatory. In facts, the necessity of heave actuation at the tracking point is due to the initial attitude of the vehicle and its need to instantaneously move along the $z_o$ axis. If the vehicle had a 90° initial pitch angle and were passively stable in this configuration then surge actuation would be sufficient to fulfill the task. Moreover, this application is a trajectory tracking scenario, the vehicle is supposed to track the position and velocity of the target at all time. It is not allowed any uncontrolled transitory regime as it could be the case in setpoint regulation scenarios. In such cases, the tracking performances would be degraded locally but heave actuation as such would not be necessary. This simple example also demonstrates the impact of the initial condition of a task or, more broadly, the implications of having subsequent fairly simple pieces of trajectory which, when put together, form a more complex task.

Table 3.1 gives two solutions: a tracking point on the $x_B$ axis with surge and pitch actuation or a tracking point on the $y_B$ axis with sway and roll actuation. In the first
solution, the coordinates of the tracking point are $[\epsilon \ 0 \ 0]^\top$. The corresponding vectors of actuated and controlled DOF as well as the $\mathcal{H}$ matrix used in this case are:

$$h_{O_B}^1 = \begin{bmatrix} 1 \\ 0 \\ 0 \\ 0 \\ 1 \\ 0 \end{bmatrix} h_E = \begin{bmatrix} 1 \\ 0 \\ 1 \\ 0 \\ 0 \\ 0 \end{bmatrix} \mathcal{H}_1 = \begin{bmatrix} 1 & 0 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 1/\epsilon & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 \end{bmatrix}$$

In the second solution, the coordinates of the tracking points are $[0 \ \epsilon \ 0]^\top$ and the vectors of actuated and controlled DOF as well as the $\mathcal{H}$ matrix are:

$$h_{O_B}^2 = \begin{bmatrix} 0 \\ 1 \\ 0 \\ 1 \\ 0 \\ 0 \end{bmatrix} h_E = \begin{bmatrix} 0 \\ 1 \\ 1 \\ 0 \\ 0 \\ 0 \end{bmatrix} \mathcal{H}_2 = \begin{bmatrix} 1 & 0 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 1/\epsilon & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 \end{bmatrix}$$

In this example, the tracking point is outside of the hull for the vehicle to keep kinematic equivalence with the first case. It also allows showing that the tracking point does not need to be a physical point of the vehicle. Any point of $\mathcal{R}_B$ can be chosen as a tracking point.

The different behaviors created by the two control strategies appear clearly on the simulation snapshot figure 3.1. On figure 3.1, each colored cylinder represents the hull of a vehicle. The blue one uses pitch compensation while the red one uses roll compensation. They are both equipped with two thrusters represented with grey cylinders. The tracked trajectory is represented with the dashed black line and the tracking point of the two vehicle are figured with a black circle. Both tracking point overlap with the current target
<figure><img src="image_17.png" /><figcaption>Figure 3.1 – Simulation snapshot at *t* = 1s. Blue vehicle: Pitch compensation - Red vehicle: Roll compensation</figcaption></figure>
<figure><img src="image_18.png" /><figcaption>Figure 3.2 – Comparison of the position of the vehicles in the two cases. Blue: Pitch compensation - Red: Roll compensation. a: $\boldsymbol{x}_0$, b: $\boldsymbol{y}_0$, c: $\boldsymbol{z}_0$</figcaption></figure>

on this snapshot showing perfect position tracking in both cases.

Figure 3.2 shows that the two compensating solutions are equivalent in terms of position tracking and notably regarding the $\boldsymbol{z}_0$ axis. On the other hand, figure 3.3 shows the difference of strategy of the two compensation solutions. The first vehicle (Blue line) pitches down to generate a heave motion at the tracking point as shown on figure 3.3.(c) whereas the second vehicle (Red Line) rolls on the side to generate heave motion as shown on figure 3.3.(a).

This elementary example illustrates the necessary choice of strategy using this method. Also, if the task also required the vehicle to keep a neutral pitch for instance, then the second solution using roll for compensation would be preferable. On the other hand if the vehicle were equipped with a camera pointing downwards then roll should not be disturbed and the first solution should be chosen. Additional constraints would limit the possible solutions. The vehicle being suitably equipped, it is also possible to alternate between two strategies during the mission, the needed $\mathcal{H}$ matrix being recalculated to meet the chosen strategy with the algorithm presented in the following.
<figure><img src="image_19.png" /><figcaption>Figure 3.3 – Comparison of the orientation of the vehicles in the two cases. Desired orientation values are arbitrarily put to 0 but are not part of the task. Blue: Pitch compensation - Red: Roll compensation. a: Roll, b: Pitch, c: Yaw</figcaption></figure>
### 3.2.3 Algorithm for the calculation of $\mathcal{H}$

This section details the algorithm calculating the Handy matrix $\mathcal{H}$ for a given system and task. The computation of $\mathcal{H}$ makes use of the skew-symmetrical cross-product matrix $\mathbf{S}(\boldsymbol{\lambda})$. Notably, for point $\boldsymbol{E}$ of coordinates $[\varepsilon_x \ \varepsilon_y \ \varepsilon_z]^\top$, it calculates $\mathbf{S} = \mathbf{S}([1/\varepsilon_x \ 1/\varepsilon_y \ 1/\varepsilon_z]^\top)$:

$$ \mathbf{S} \begin{pmatrix} 1/\varepsilon_x \\ 1/\varepsilon_y \\ 1/\varepsilon_z \end{pmatrix} = \begin{bmatrix} 0 & -1/\varepsilon_z & 1/\varepsilon_y \\ 1/\varepsilon_z & 0 & -1/\varepsilon_x \\ -1/\varepsilon_y & 1/\varepsilon_x & 0 \end{bmatrix} \qquad (3.19) $$

Note that, to avoid singularities, the matrix $\mathbf{S}$ is built only with the non-zero coordinates of $\boldsymbol{E}$. As an example, for a tracking point on the $x_B$ axis $\varepsilon_x \neq 0$ and $\varepsilon_y = \varepsilon_z = 0$, one would build $\mathbf{S}([1/\varepsilon_x \ 0; 0]^\top)$ replacing $1/\varepsilon_y$ and $1/\varepsilon_z$ with 0.

<table><caption>**Algorithm 1** Calculation of the Handy matrix $\mathcal{H}$</caption><tr><td>$\mathcal{H} \leftarrow \mathbb{I}_6$</td></tr><tr><td>$\mathbf{e} \leftarrow [\varepsilon_x \ \varepsilon_y \ \varepsilon_z]^\top$</td></tr><tr><td>$\boldsymbol{\epsilon} \leftarrow [0 \ 0 \ 0]^\top$</td></tr><tr><td>for $k = 1 : 3$</td></tr><tr><td>&nbsp;&nbsp;&nbsp;&nbsp;if $\mathbf{e}(k) \neq 0$</td></tr><tr><td>&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;$\boldsymbol{\epsilon}(k) \leftarrow 1/\mathbf{e}(k)$</td></tr><tr><td>$\boldsymbol{\Sigma} \leftarrow \mathbf{S}(\boldsymbol{\epsilon})$</td></tr><tr><td>for $i = 3 : 6$</td></tr><tr><td>&nbsp;&nbsp;&nbsp;&nbsp;if $\mathbf{h}_{O_B}(i) = 1$ and $\mathbf{h}_E(i) = 0$</td></tr><tr><td>&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;for $j = 1 : 3$</td></tr><tr><td>&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;if $\mathbf{h}_E(j) = 1$ and $\mathbf{h}_{O_B}(j) = 0$ and $j \neq i - 3$</td></tr><tr><td>&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;$\mathcal{H}(i, j) \leftarrow \Sigma(i - 3, j)$</td></tr><tr><td>&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;$\mathcal{H}(j, :) \leftarrow 0$</td></tr></table>

Algorithm 1 takes the two vectors $\mathbf{h}_{O_B}$ and $\mathbf{h}_E$ as inputs as well as the coordinates of the tracking point $\boldsymbol{E}$. The user needs to define the actuated DOF of the vehicle in $\mathbf{h}_{O_B}$ and choose, following the design rules mentioned before, the controlled DOF at the tracking point in $\mathbf{h}_E$.

Overall, algorithm 1 browses through the actuated rotational DOF of the vehicle. If
it finds a rotation in $h_{O_B}$ which is not controlled in $h_E$, it checks whether it is used to compensate for a translation. When the rotation and translation are found, the appropriate ratio $1/\epsilon_k$ ($\epsilon_k$ being the k-th coordinate of $E$ in $\mathcal{R}_B$) is selected in $\Sigma$ and placed at the corresponding place in the bottom left submatrix of $\mathcal{H}$. Finally, it cancels the row of $\mathcal{H}$ corresponding to the non actuated translation of $h_{O_B}$.

For demonstration of the matrix generation, let us apply algorithm 1 to the uqr-vessel example (3.18) introduced in the previous section. In this example, the $\mathcal{H}$ matrix must compensate the lack of two translations, sway and heave, with pitch and yaw. Considering a tracking point $E$ of coordinates $[\epsilon_x 0 0]^T$, the skew-symmetrical cross-product matrix of interest is:

$$ \Sigma = S \begin{pmatrix} \begin{bmatrix} \frac{1}{\epsilon_x} \\ 0 \\ 0 \end{bmatrix} \end{pmatrix} = \begin{bmatrix} 0 & 0 & 0 \\ 0 & 0 & -\frac{1}{\epsilon_x} \\ 0 & \frac{1}{\epsilon_x} & 0 \end{bmatrix} $$

The matrix $\mathcal{H}$ is initialized with the identity.

$$ h_{O_B}^1 = \begin{bmatrix} 1 \\ 0 \\ 0 \\ 0 \\ 1 \\ 0 \end{bmatrix} \qquad h_E = \begin{bmatrix} 1 \\ 0 \\ 1 \\ 0 \\ 0 \\ 0 \end{bmatrix} \qquad \mathcal{H} = \begin{bmatrix} 1 & 0 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 \end{bmatrix} $$

Considering the pair $h_{O_B}$, $h_E$ of (3.18), the first rotational DOF actuated in $h_{O_B}$ but not controlled in $h_E$ is pitch (fifth line, $i=5$). The algorithm now seeks a translation in $h_E$ which is not actuated in $h_{O_B}$ and can be compensated ($j \neq i - 3$) and finds one on the third line ($j=3$) corresponding to heave. The appropriate term of $\Sigma$ (second row, third column) is placed in the appropriate position in $\mathcal{H}$ (fifth row, third column) and the
heave line of $\mathcal{H}$ is canceled. At this stage $\mathcal{H}$ is:

$$ \mathcal{H} = \begin{bmatrix} 1 & 0 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & -\frac{1}{\varepsilon_x} & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 \end{bmatrix} $$

Then, the algorithm resumes, reading $h_{o_B}$ to find another rotation used for compensation. Yaw is used for compensation of sway ($i = 6, j = 4$) so the appropriate term of $\mathcal{S}$ is again copied in $\mathcal{H}$ and the sway line of $\mathcal{H}$ is canceled. At this stage the handy matrix for this example is:

$$ \mathcal{H} = \begin{bmatrix} 1 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & -\frac{1}{\varepsilon_x} & 0 & 1 & 0 \\ 0 & \frac{1}{\varepsilon_x} & 0 & 0 & 0 & 1 \end{bmatrix} \qquad (3.20) $$

Finally equation (3.20) shows the handy matrix $\mathcal{H}$ for an uqr-vessel navigating on a $(x, y, z)$ path and with a tracking point at the front of the vehicle. This algorithm can be applied for any suitable pair $h_{o_B}, h_E$.

## 3.3 Application to the RSM Robot

In this section, the $\mathcal{H}$-based controller is detailed and adapted to the RSM robot. The RSM robot is an underactuated underwater vehicle used in several research applications over the years [31, 30]. This vehicle is used in this example as a representative of a class of
<figure><img src="image_20.png" /><figcaption>Figure 3.4 – The propulsive configuration of the RSM robot</figcaption></figure>

underactuated marine craft lacking sway and pitch actuation. The propulsive arrangement
of this vehicle is described in section 1.2.1.2 and a 3D rendering appears on figure 3.4.
The actuation of RSM is described here in terms of its associated vectors $\mathbf{h}_{\mathbf{O}_B}$ and $\mathbf{h}_E$. A
couple of possible control strategies are described. The complete model-based controller is
given in details and a convergence demonstration of the $\mathcal{H}$-based controller is presented.

**3.3.1 Actuation and task of the RSM Robot**

The *RSM* Robot is composed of a cylindrical hull and is equipped with four thrusters.
The first two thrusters $P_1$ and $P_2$ are in the plane ($\mathbf{O}_B, \mathbf{x}_B, \mathbf{y}_B$) at the rear of the vehicle
aligned with $\mathbf{x}_B$. The two other thrusters are placed in the ($\mathbf{O}_B, \mathbf{y}_B, \mathbf{z}_B$) plane and are
aligned with $\mathbf{z}_B$. Therefore, the vehicle is able to generate independent surge and heave
forces as well as roll and yaw moments. Sway and pitch are not actuated. Nevertheless,
because the center of gravity $\mathbf{P}_G$ is slightly below the center of buoyancy $\mathbf{P}_B = \mathbf{O}_B$, the
vehicle is naturally stable in roll and pitch.
<figure><img src="image_21.png" /><figcaption>Figure 3.5 – Top view of the seabed scanning trajectory at 1 m depth.</figcaption></figure>

The actuation vector for RSM is:

$$h_{OB} = \begin{bmatrix} 1 \\ 0 \\ 1 \\ 1 \\ 0 \\ 1 \end{bmatrix} \qquad (3.21)$$

The tracking point is chosen at the bow of the vehicle below the waterline. The co-ordinates of E are $[\epsilon_x \ 0 \ \epsilon_z]^T$ and for this example we take $\epsilon_x = \epsilon_z = 0.2$ m. As exposed before, the position of the tracking point adds to the stability of the vehicle and is chosen to create exploitable kinematic couplings.

The vehicle is evaluated on the *Seabed Scanning* task presented on figure 3.5. This task is composed of several horizontal rails at constant depth and the vehicle must stay
tangent to the rails. Therefore, the required DOF for this task are the three translations and the rotation around $z_o$, the vehicle is ill-actuated. No additional angular constraints are added to the task.

Because the required DOF of the task are different from the actuated DOF of the vehicle, a compensation mechanism is introduced in the controller to cope for the lack of sway actuation. Referring to table 3.1, two solutions raise for this tracking point and propulsive arrangement: either use roll and heave or use yaw and surge. This last solution is the most commonly used on usual underactuated surface vessels.

The two strategies are associated with two different vectors for the controlled DOF at the tracking point:

$$h_{E,p} = \begin{bmatrix} 1 \\ 1 \\ 1 \\ 0 \\ 0 \\ 1 \end{bmatrix} \quad \text{and} \quad h_{E,r} = \begin{bmatrix} 1 \\ 1 \\ 1 \\ 1 \\ 0 \\ 0 \end{bmatrix}$$

In the first case $h_{E,p}$, roll is used for sway compensation so the yaw angle of the vehicle can be controlled. In the second case $h_{E,r}$, yaw is used for compensation and can thus not be controlled. In this case though roll, which is not part of the task, could be controlled. Note that, because of the hydrodynamic effects on the hull of the vehicle, the yaw angle is naturally stabilized at non-zero speed and the hull of the vehicle naturally aligns with the velocity vector. Hence, only minimal yaw disturbance appear in this task when it is not controlled. To see a significant difference between the two strategies, additional tests will be conducted with a yaw constraint different from the tangent to the track.

Of course, the two vectors $h_{E,p}$ and $h_{E,r}$ correspond to two different matrices $\mathcal{H}_p$ and $\mathcal{H}_r$ respectively. Applying algorithm 1 in the two cases with $h_{O_B}$ defined as in (3.21)
yields:

$$ \mathcal{H}_p = \begin{bmatrix} 1 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 & 0 \\ 0 & -1/\epsilon_z & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 \end{bmatrix} \qquad (3.22) $$

and

$$ \mathcal{H}_r = \begin{bmatrix} 1 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 1/\epsilon_x & 0 & 0 & 0 & 1 \end{bmatrix} \qquad (3.23) $$

In order to demonstrate the action of these two matrices, they are applied to the relation (3.15) in the same way as in the theoretical example of section 3.1. Inverting equation (3.15) gives:

$$ \nu = T^{-1}\nu_E \qquad (3.24) $$

The Handy matrices $\mathcal{H}_p$ and $\mathcal{H}_r$ can be applied to this system giving:

$$ \nu_1 = \mathcal{H}_p T^{-1} \nu_E \qquad (3.25) $$

and

$$ \nu_2 = \mathcal{H}_r T^{-1} \nu_E \qquad (3.26) $$
The first case is detailed as:

$$
\begin{bmatrix} u \\ v \\ w \\ p \\ q \\ r \end{bmatrix} = \begin{bmatrix} 1 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 & 0 \\ 0 & -1/\epsilon_z & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} 1 & 0 & 0 & 0 & -\epsilon_z & 0 \\ 0 & 1 & 0 & \epsilon_z & 0 & -\epsilon_x \\ 0 & 0 & 1 & 0 & \epsilon_x & 0 \\ 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} u_E \\ v_E \\ w_E \\ p_E \\ q_E \\ r_E \end{bmatrix} = \begin{bmatrix} u_E - \epsilon_z q_E \\ 0 \\ w_E + \epsilon_x q_E \\ v_E - \epsilon_x r_E \\ q_E \\ r_E \end{bmatrix} \quad (3.27)
$$

Equations (3.27) shows that, because of the off-diagonal term of $\mathcal{H}_p$, the roll speed $p$ is now a function of the sway speed at point $E$, $v_E$. Same reasoning can be applied with $\mathcal{H}_r$, making the yaw speed $r$ a function of the sway speed at point $E$, $v_E$. The next section explains how this new behavior can be used in a controller to allow tracking of a non-actuated DOF.

### 3.3.2 Complete controller

This section introduces the complete controller used on the RSM Robot in association with the $\mathcal{H}$ matrix-based guidance principle. Some variations of this controller have been introduced in chapter 2. The controller is a two-staged cascade system. A velocity reference is calculated with a PI control law and the $\mathcal{H}$ matrix, to cancel the position errors. A model-based proportional effort vector is then proposed to control the speed of the vehicle towards the reference speed.

The control effort vector $\tau_c$ is calculated as:

$$ \tau_c = M(\dot{v}_c + K(v_c - v)) + C(v)\nu + D(v)\nu + g(\eta) \quad (3.28) $$

where $v_c$ is the reference velocity vector calculated with $\mathcal{H}$ and detailed in the following and $K$ is an appropriate proportional gain matrix.

Equation (3.28) shows a typical feedback linearizing structure associated with a proportional controller [65]. The calculation of $\tau_c$ is directly drawn from the dynamic model of the vehicle equation (1.5). Matrices $M$, $C(v)$ and $D(v)$ are the mass, Coriolis and centripetal and damping matrices of the model respectively and $g(\eta)$ is the vector of
<figure><img src="image_22.png" /><figcaption>Figure 3.6 – Block diagram of the cascade $\mathcal{H}$-based controller. $\Sigma$ represents the AUV, $N(\eta, \nu) = C(\nu)\nu + D(\nu)\nu + g(\eta)$</figcaption></figure>

gravity and buoyancy effects. The non linear terms of the model are evaluated at the current state of the system $(\eta, \nu)$ therefore allowing feedback linearization; Applying the control vector $\tau_c$ cancels the non linear terms of the model equation (1.5), leading to the linear closed-loop system:

$$ \dot{\nu} = \dot{\nu}_c + K(\nu_c - \nu) \qquad (3.29a) $$

$$ e_{\nu} = (\nu_{c} - \nu) \qquad (3.29b) $$

$$ \dot{e}_{\nu} + Ke_{\nu} = 0 \qquad (3.29c) $$

The new velocity reference $\nu_c$ is also built as a model-based feedback linearizing controller but, in this case, including the Handy matrix $\mathcal{H}$ to create the compensation behavior. Recalling the kinematic model of the vehicle including the tracking point $E$ equation (1.3) one gets:

$$ \nu_c = \mathcal{H}T^{-1}J(\eta)^{-1} \left[ \dot{\eta}^* + K_p e_{\eta} + K_i \int_0^t e_{\eta}(\zeta)d\zeta \right] \qquad (3.30) $$

with $e_{\eta} = \eta^* - \eta_E$ the error between the desired state $\eta^*$ and the state of the tracking point $\eta_E$.

Overall, this controller is a *cascade system* as depicted on figure 3.6. The inner loop (dynamic stage), calculates the efforts $\tau_c$ required for the vehicle to reach the velocity reference $\nu_c$ calculated by the outer loop (kinematic guidance). The outer loop therefore behaves like a kinematic guidance stage outputting new reference velocities for the controller in a similar way as a LOS guidance stage or any other guidance algorithm would.
Cascade systems are known for their natural robustness to external disturbances but the inner loop must be faster than the outer loop to ensure stability.

Note that, in equation (3.30) the PI control law could be replaced by any usual controllers (SMC, LQR). Using a different control law in the outer loop could produce different behavior. Notably, adding a sliding surface and switching term would enhance robustness to external disturbances.

### 3.3.3 Stability proof

To show the convergence of the controller, this section focuses on the second Handy Matrix solution $\mathcal{H}_r$ applied to the RSM robot in a planar trajectory tracking application. This example allows demonstrating that the Handy matrix indeed allows convergence of the compensated DOF and does not disturb the other controlled DOF. It could be reproduced for any other actuation configuration, task, or compensated DOF.

The cascade structure of the controller and the feedback linearizing PI-based forms used in both stages give a first hint towards stability of the controller. Yet, stability of the overall closed-loop system is addressed here to illustrate the behavior of the controller not only with respect to the compensated DOF but also regarding the other DOF not involved in the compensation.

Let us recall the complete model of the underwater vehicle:

$$ \tau = M\dot{\nu} + C(\nu)\nu + D(\nu)\nu + g(\eta) \quad (3.31a) $$

$$ \dot{\eta}_E = J(\eta)T\nu \quad (3.31b) $$

with, because of underactuation of the RSM robot, the vector of propulsive efforts $\tau$ of shape: $\tau = [X \ 0 \ Z \ K \ 0 \ N]^T$ ($X$ and $Z$ being non-zero force components and $K$ and $M$ being non-zero moments).

A tracking point $E$ of coordinates $[\varepsilon_x \ 0 \ 0]^T$ in $\mathcal{R}_B$ is used in this proof. The associated
transformation matrix is:

$$ T = \begin{bmatrix} 1 & 0 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 & \epsilon_x \\ 0 & 0 & 1 & 0 & -\epsilon_x & 0 \\ 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 \end{bmatrix} \qquad (3.32) $$

The sway of the vehicle is not actuated but can be compensated with yaw. The corresponding Handy matrix $\mathcal{H}_r$ calculated with algorithm 1 is given in equation (3.23) and recalled here:

$$ \mathcal{H}_r = \begin{bmatrix} 1 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 1/\epsilon_x & 0 & 0 & 0 & 1 \end{bmatrix} \qquad (3.33) $$

As detailed in section 3.3.2, the controller used in this example is:

$$ \tau_c = M(\dot{\nu}_c + Ke_\nu) + C(\nu)\nu + D(\nu)\nu + g(\eta) \qquad (3.34a) $$

$$ \nu_c = \mathcal{H}_r T^{-1} J(\eta)^{-1} (\dot{\eta}^* + \lambda(e_\eta)) \qquad (3.34b) $$

In equation (3.34b), $\lambda(e_\eta)$ is a PI control law based on the tracking error $e_\eta$:

$$ \lambda(e_\eta) = K_p e_\eta + K_i \int_0^t e_\eta(\zeta) d\zeta \qquad (3.35) $$

Following the results of appendix C and as demonstrated before, thanks to the feedback linearizing terms, applying the control vector $\tau_c$ to the dynamic model (3.31a) cancels the non-linearities of the dynamic model on the actuated DOF. The non actuated DOF (sway and pitch) are naturally stable and, because no coupled efforts are generated by
the actuators on these two axes, they are assumed to stay stable in closed loop with reasonable disturbances.

**Remark 3.1** Rigorously, the following equations should only be written for the controlled DOF but for the sake of clarity and considering the natural sway and pitch stability of the system, the equations are given on the complete system.

The effort control vector $\tau_c$ creates the linear closed-loop system:

$$ \dot{\nu} = \dot{\nu}_c + K(\nu_c - \nu) \qquad (3.36) $$

Now, in order to give a demonstration of the convergence of the position errors in $\mathcal{R}_0$, equation (3.36) must be rewritten using the following relations drawn from equation (3.31b) and equation (3.34b).

$$ \dot{\nu} = T^{-1}(\dot{\mathbf{J}}(\eta, \dot{\eta})^{-1}\dot{\eta}_E + \mathbf{J}(\eta)^{-1}\ddot{\eta}_E) \qquad (3.37a) $$

$$ \dot{\ddot{\eta}} = \dot{\eta}^* + \lambda(e_\eta) \qquad (3.37b) $$

$$ \nu_c = \mathcal{H}_r T^{-1} \mathbf{J}(\eta)^{-1} \dot{\eta} \qquad (3.37c) $$

$$ \dot{\nu}_c = \mathcal{H}_r T^{-1} (\dot{\mathbf{J}}(\eta, \dot{\eta})^{-1} \dot{\eta} + \mathbf{J}(\eta)^{-1} \ddot{\eta}) \qquad (3.37d) $$

The gain matrices of the controller are defined as:

$$ \mathbf{K} = K\mathbb{I} \qquad (3.38a) $$

$$ \mathbf{K}_p = k_p\mathbb{I} \qquad (3.38b) $$

$$ \mathbf{K}_i = k_i\mathbb{I} \qquad (3.38c) $$

where $\mathbb{I}$ is the identity matrix. It implies that the sets of gain parameters are the same on all the DOF. Each independent gain could be tuned to better match the individual characteristics of each DOF without changing the convergence result.

Using the expressions of (3.37) in equation (3.36) gives:

$$ \mathcal{H}_r T^{-1} (\dot{\mathbf{J}}(\eta, \dot{\eta})^{-1} \dot{\eta}^* + \mathbf{J}(\eta)^{-1} \ddot{\eta}^*) + \mathbf{K} (\mathcal{H}_r T^{-1} \dot{\mathbf{J}}(\eta, \dot{\eta})^{-1} \dot{\eta}^* - T^{-1} \mathbf{J}(\eta)^{-1} \dot{\eta}_E) = T^{-1} (\dot{\mathbf{J}}(\eta, \dot{\eta})^{-1} \dot{\eta}_E + \mathbf{J}(\eta)^{-1} \ddot{\eta}_E) \qquad (3.39) $$
Multiplying both sides by $T$ leads to:

$$ T H_r T^{-1} (\dot{J}(\eta, \dot{\eta})^{-1} \dot{\eta}^* + J(\eta)^{-1} \ddot{\eta}^*) + T K (H_r T^{-1} \dot{J}(\eta, \dot{\eta})^{-1} \dot{\eta}^* - T^{-1} J(\eta)^{-1} \dot{\eta}_E) = \dot{J}(\eta, \dot{\eta})^{-1} \dot{\eta}_E + J(\eta)^{-1} \ddot{\eta}_E \quad (3.40) $$

which can then be rewritten as:

$$ T H_r T^{-1} (\dot{J}(\eta, \dot{\eta})^{-1} \dot{\eta}^* + J(\eta)^{-1} \ddot{\eta}^*) + K (T H_r T^{-1} \dot{J}(\eta, \dot{\eta})^{-1} \dot{\eta}^* - J(\eta)^{-1} \dot{\eta}_E) = \dot{J}(\eta, \dot{\eta})^{-1} \dot{\eta}_E + J(\eta)^{-1} \ddot{\eta}_E \quad (3.41) $$

The matrix product $T H_r T^{-1}$ reads:

$$ T H_r T^{-1} = \begin{bmatrix} 1 & 0 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 1/\epsilon_x & 0 & 0 & 0 & 0 \end{bmatrix} \quad (3.42) $$

and, considering that the roll and pitch are stable and undisturbed, the vehicle is supposed to stay in the horizontal plane where $\phi \approx 0$ and $\theta \approx 0$. Matrices $J(\eta)^{-1}$ and $\dot{J}(\eta, \dot{\eta})^{-1}$
are then:

$$ \mathbf{J}(\eta)^{-1} = \begin{bmatrix} c\psi & s\psi & 0 & 0 & 0 & 0 \\ -s\psi & c\psi & 0 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 \end{bmatrix} \qquad (3.43a) $$

$$ \dot{\mathbf{J}}(\eta, \dot{\eta})^{-1} = \begin{bmatrix} -\dot{\psi}s\psi & \dot{\psi}c\psi & 0 & 0 & 0 & 0 \\ -\dot{\psi}c\psi & -\dot{\psi}s\psi & 0 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 \end{bmatrix} \qquad (3.43b) $$

Considering equation (3.42) and equation (3.43) and after some calculations, the two first lines of equation (3.41) give in the earth-fixed frame:

$$ \ddot{e}_x + (k_p + K)\dot{e}_x + (k_i + Kk_p)e_x + Kk_i\chi_x + \dot{\psi}(\dot{e}_y + k_p e_y + k_i\chi_y) = 0 \qquad (3.44a) $$

$$ \ddot{e}_y + (k_p + K)\dot{e}_y + (k_i + Kk_p)e_y + Kk_i\chi_y - \dot{\psi}(\dot{e}_x + k_p e_x + k_i\chi_x) = 0 \qquad (3.44b) $$

with $\chi_x = \int_0^t e_x(\zeta)d\zeta$ and $\chi_y = \int_0^t e_y(\zeta)d\zeta$.

Remark that the sixth equation of (3.41) gives the dynamics of the orientation parameter $\psi$, not controlled in this case:

$$ \ddot{\psi} = \frac{1}{\epsilon_x} \left[ -\dot{\psi}(\dot{x}^*c\psi + \dot{y}^*s\psi) - \ddot{x}s\psi + \ddot{y}c\psi + K(-\dot{x}^*s\psi + \dot{y}^*c\psi) \right] - K\dot{\psi} \qquad (3.45) $$

Equation (3.45) has the shape of a Liénard equation whom study is outside of the scope of this work.
### 3.3.3.1 Lyapunov argument

The stability of the closed-loop system (3.44) is demonstrated in this Section using Lyapunov theory. First, new error variables are introduced:

$$ \sigma_x = \dot{e}_x + k_p e_x + k_i \chi_x \quad (3.46a) $$

$$ \sigma_y = \dot{e}_y + k_p e_y + k_i \chi_y \quad (3.46b) $$

The two error variables $\sigma_x$ and $\sigma_y$ are similar to the standard definition of sliding surfaces (see section 2.4) but are just used as intermediate variables in the present demonstration. The first order derivatives of these two new error variables are:

$$ \begin{aligned} \dot{\sigma}_x &= -(k_p + K)\dot{e}_x - (k_i + Kk_p)e_x - Kk_i\chi_x - \dot{\psi}(\dot{e}_y + k_p e_y + k_i \chi_y) + k_p \dot{e}_x + k_i e_x \\ &= -K\sigma_x - \dot{\psi}\sigma_y \end{aligned} \quad (3.47a) $$

$$ \begin{aligned} \dot{\sigma}_y &= -(k_p + K)\dot{e}_y - (k_i + Kk_p)e_y - Kk_i\chi_y + \dot{\psi}(\dot{e}_x + k_p e_x + k_i \chi_x) + k_p \dot{e}_y + k_i e_y \\ &= -K\sigma_y + \dot{\psi}\sigma_x \end{aligned} \quad (3.47b) $$

**Remark 3.2** At this point and to stick with the concept of SMC, choosing an appropriate sliding condition would also allow demonstrating that an additional SMC-based stage of control would add robustness to external bounded disturbances.

A natural Lyapunov function candidate is:

$$ V = \frac{1}{2}(\sigma_x^2 + \sigma_y^2) \quad (3.48) $$

Function $V$ is of class $C^1$, positive definite and radially unbounded. The first order time derivative of the Lyapunov function candidate is:

$$ \dot{V} = \dot{\sigma}_x \sigma_x + \dot{\sigma}_y \sigma_y = -K\sigma_x^2 - \dot{\psi}\sigma_x \sigma_y - K\sigma_y^2 + \dot{\psi}\sigma_x \sigma_y = -K(\sigma_x^2 + \sigma_y^2) \quad (3.49) $$

Then, for any choice of $K$ strictly positive, $\dot{V}$ is strictly negative and $\dot{V} \le -\alpha V$ with $\alpha \ge 2K$. Consequently, the equilibrium point $\sigma_x = 0, \sigma_y = 0$ is globally exponentially stable (GES). Also, owing to the cascade structure of equation (3.46), the equilibrium $e_x = 0, e_y = 0$ is then GES too.

One interesting remark is that the GES character of the method is independent from
any condition on the yaw rate of the vehicle. This property is of particular interest in controlling the AUV planar trajectory, considering that the dynamics of $\psi$ given by equation (3.45) are hardly predictable or controllable.

## 3.4 Simulation results

This section shows trajectory tracking results obtained in simulation on the RSM robot using the controller based on the Handy matrix $\mathcal{H}$. Several tests are conducted; First, the two possible compensation solutions $\mathcal{H}_p$ and $\mathcal{H}_r$ (section 3.3.1) are compared on the Seabed Scanning task. Then, the $\mathcal{H}_r$ solution is compared with a traditional LOS controller and a SM controller on the same task with and without sea current.

### 3.4.1 Comparison of compensation solutions

#### 3.4.1.1 Seabed Scanning

First, the two different compensation solutions $\mathcal{H}_p$ and $\mathcal{H}_r$ are compared on the seabed scanning task using a PI control law in the outer loop and a P control law in the inner loop. Table 3.2 gives the simulation parameters used for this experiment as well as the position of the tracking point and the gain parameters used for both vehicles.

<table><caption>Table 3.2 – Simulation parameters - seabed scanning mission</caption><thead><tr><th>Item</th><th>Value</th><th>Item</th><th>Value</th></tr></thead><tbody><tr><td>Trajectory</td><td>Seabed Scanning</td><td>Nominal Speed</td><td>1.5m.s<sup>-1</sup></td></tr><tr><td>Current velocity</td><td>[0 0 0]<sup>T</sup></td><td>Simulation time sample</td><td>0.05ms</td></tr><tr><td>Vehicle Shape</td><td>Cylinder</td><td>Propulsive configuration</td><td>RSM</td></tr><tr><td>Tracking point</td><td>[0.2 0 0.2]<sup>T</sup></td><td>K</td><td>8</td></tr><tr><td>k<sub>p</sub></td><td>8</td><td>k<sub>i</sub></td><td>2</td></tr></tbody></table>

Figure 3.7 displays the position and orientation of both vehicles with each compensation strategy and shows that both solutions have equally good position tracking performances on the three axes. This confirms that the method produces the desired results
<figure><img src="image_23.png" /><figcaption>Figure 3.7 – Comparison of the position and orientation of the two vehicles. Blue: $\mathcal{H}_p$, Red: $\mathcal{H}_r$, Dashed: Reference</figcaption></figure>

in terms of position tracking regardless of the chosen compensation strategy. However, figure 3.7 shows differences in the orientation of both vehicles as highlighted in figure 3.8.

This last figure, compares the orientation errors between the two solutions. Here, notable differences raise. First, figure 3.8.(a) shows that the first compensation strategy creates a larger roll disturbance than the second one. This results was expected since this DOF is used for compensation in the first case and can therefore not be controlled while it is controlled in the second case. Yet, this greater disturbance is acceptable since roll is not part of the required DOF of the task. For the same reason, figure 3.8.(c) shows a larger error on the yaw angle when the second solution is used. In this case, yaw is used for compensation and is not controlled. It leads to approximately 25° errors in curves. This error is naturally canceled on the straight lines because of the hydrodynamic drag which realigns the vehicle on the global speed.

### 3.4.1.2 Rail on the $y_0$ axis with yaw constraint

The differences between the two strategies appear more clearly on figure 3.9. This time the vehicle is evaluated on a single rail aligned with the $y_0$ axis at constant depth but with the additional yaw constraint of staying perpendicular to the trajectory. The desired yaw angle is $\psi^* = 0$ during all the task. Once again, the method shows very good tracking results in position for both strategies. However, the first vehicle using matrix $\mathcal{H}_p$ generates a roll angle of approximately 70° degrees but has a perfect yaw tracking
<figure><img src="image_24.png" /><figcaption>Figure 3.8 – Comparison of orientation errors for the two compensation solutions on the Seabed Scanning. Blue: $\mathcal{H}_p$, Red: $\mathcal{H}_r$ – a: Roll error – b: Pitch error – c: Yaw error</figcaption></figure>
<figure><img src="image_25.png" /><figcaption>Figure 3.9 – Comparison of orientation errors for the two compensation solutions on the $\mathbf{y}_o$ rail with $\psi^* = 0$. Blue: $\mathcal{H}_p$, Red: $\mathcal{H}_r$ - a: Roll error - b: Pitch error - c: Yaw error</figcaption></figure>
while the second vehicle using matrix $\mathcal{H}_r$ generates a yaw error of 90°. Here again the second vehicle neglects the desired yaw and stabilizes along the linear velocity vector of the trajectory.

This last case study shows two different things. First, it demonstrates that some strategies can provide completely different results on different tasks. In this case, the yaw compensation solution using $\mathcal{H}_r$ can be considered acceptable on the first task (figure 3.7) but is totally not suited for the second task (figure 3.9). It demonstrates that the combination of trajectory, constraints and natural dynamics of the vehicle is central in the definition of the task. In the first task, the yaw angle is only slightly disturbed by the controller and hydrodynamic effects cause it to tend naturally towards the desired value. In the definition of the trajectory, choosing a desired yaw angle equal to the tangent to the desired velocity vector does match the natural dynamics of the vehicle. Depending on the application, a little disruption of the yaw angle during the turns can be accepted. On the second task however, the yaw constraint goes radically against the natural dynamics of the vehicle. In this case, the yaw angle of the vehicle must be actively controlled to meet the constraints imposed by the task. Comparing these two task shows that, even if they theoretically require the same DOF (surge, sway, heave and yaw), the association of the trajectory constraints and natural dynamics of the chosen vehicle implies two different control strategies.

The second effect demonstrated by these two case studies is the possible disturbance created by the chosen compensation strategy. The yaw angle constraint has been added to keep the vehicle pointed perpendicularly to a vertical plane of a virtual object in the spirit of a sensor inspection of a submerged structure. In such a case, the constraint is not really on the yaw angle of the vehicle but is rather on the orientation of the $x_B$ axis of the vehicle with respect to an object in the inertial frame. Thus, to maintain the desired orientation, the vehicle must be able to control the orientation of its longitudinal axis around the vertical axis of the inertial plane, $z_o$. Yet, figure 3.9 shows that the roll compensation strategy can create quite drastic roll angles depending on the required sway and heave speeds. Here, the roll of the vehicle stabilizes around 70° but the value of the equilibrium depends on the desired speeds of the trajectory. For a greater desired speed, if the vehicle had a slightly lower buoyancy or a center of gravity closer to its center of buoyancy, the roll equilibrium found by the controller would tend to 90°. The issue is that, at $\phi = 90°$, the RSM propulsive arrangement is unable to generate any moment around the vertical axis of the inertial frame $z_o$ because the two rear thrusters end up in
a vertical plane. Despite the fact that the vehicle and control strategy theoretically allow good control of all the required DOF of the task, the choice of the compensation strategy can create attitudes in which the vehicle loses control over one or several other DOF. This case of study may seem far fetched but similar issues can appear with other control laws and DOF.

### 3.4.2 Comparison with LOS and SMC

In this section, the controller based on the $H_r$ matrix is compared with a Sliding Mode Controller (SMC) drawn from [40] and a PID controller associated with Line Of Sight guidance (LOS) derived from [105]. The three controllers are evaluated on the Seabed Scanning task first without external disturbance and then in presence of an unknown constant irrotational sea current. The three controllers are applied to the *RSM* vehicle.

The LOS and SMC controllers are some of the most common in literature. They are presented in detail in section 2.1 and section 2.4 respectively. As with the $H_r$ matrix, both controllers can be used to compensate the lack of sway with yaw.

The structures of the controllers are slightly different. The LOS and $H_r$ based controllers are cascade system composed of a guidance stage as the outer loop and a feedback linearizing proportional controller as the inner loop. Whereas the SMC controller is built in a single stage. The yaw sliding surface used in the calculation of the yaw input is a function of the sway error. Equations of the dynamic model are used to calculate the input through dynamic couplings. None of the three controllers controls the yaw angle of the vehicle.

Table 3.3 shows the simulation parameters used in the two experiments. The gain parameters of the three controllers are the same in the two tests to evaluate robustness against unpredictable disturbances.

#### 3.4.2.1 Without external disturbance

Figure 3.10 shows that the position tracking performance of the three controllers are virtually identical without sea current. Figure 3.11 shows that the compensation strategies used in the three controllers do not destabilize the naturally stable DOF of the three vehicles. Notably, the yaw angle of the three vehicles naturally tends towards the tangent to the track in between the turns and is only disturbed to correct sway errors in the turns.
<figure><img src="image_26.png" /><figcaption>Figure 3.10 – Comparison of position in $\mathcal{R}_0$ for the LOS controller (Blue), the SMC (Green) and the $\mathcal{H}$ controller (Red) on the Seabed Scanning task without current. a: $\mathbf{x}_o$ axis – b: $\mathbf{y}_o$ axis – c: $\mathbf{z}_o$ axis</figcaption></figure>
<figure><img src="image_27.png" /><figcaption>Figure 3.11 – Comparison of orientation for the LOS controller (Blue), the SMC (Green) and the *H* controller (Red) on the Seabed Scanning task without current. a: Roll - b: Pitch - c: Yaw</figcaption></figure>
<figure><img src="image_28.png" /><figcaption>Figure 3.12 – Comparison of position errors for the $\mathcal{H}$ controller (1, Blue) and the SM controller (2, Red) on the Seabed Scanning with constant current on $\mathbf{x}_o$ axis. a: Error on $\mathbf{x}_o$ - b: Error on $\mathbf{y}_o$ - c: Error on $\mathbf{z}_o$</figcaption></figure>

### 3.4.2.2 Constant sea current

In the presence of a constant sea current, the tracking performance is degraded for both controllers as shown on Figs. 3.12 and 3.13. However, the performance degradation of the first vehicle (mH with kinematic PI) is probably related more to the use of a PI control law than to the guidance method. The response of the PI being quite slow, the vehicle falls behind during the turns (around 16s, 44s, 68s and 92s) but quickly catches back with the desired trajectory. All the controlled DOF converge on the trajectory between the turns showing that the guidance principle stays effective in the presence of sea current.

The SM controller of the second vehicle on the other hand (Red line on Figs. 3.12 and 3.13) has a more aggressive response during the turns which limits the error but can experience steady state error as on figure 3.13.(a).
<figure><img src="image_29.png" /><figcaption>Figure 3.13 – Comparison of orientation errors for the $\mathcal{H}$ controller (1, Blue) and the SM controller (2, Red) on the Seabed Scanning with constant current on $\mathbf{x}_o$ axis. a: Roll error - b: Pitch error - c: Yaw error</figcaption></figure>
<table><caption>Table 3.3 – Simulation parameters, with sea current</caption><thead><tr><th>Item</th><th>Value</th><th>Item</th><th>Value</th></tr></thead><tbody><tr><td>Trajectory</td><td>Seabed Scanning</td><td>Nominal Speed</td><td>1.5m.s<sup>-1</sup></td></tr><tr><td>Current velocity</td><td>[-0.75m.s<sup>-1</sup> 0 0]<sup>T</sup></td><td>Simulation time sample</td><td>0.05ms</td></tr><tr><td>Vehicle Shape</td><td>Cylinder</td><td>Propulsive arrangement</td><td>RSM</td></tr><tr><td colspan="4"><i>H</i><sub>r</sub> based controller</td></tr><tr><td>Tracking point</td><td>[0.2 0 0]<sup>T</sup></td><td><i>K</i></td><td>8</td></tr><tr><td><i>k</i><sub>p</sub></td><td>8</td><td><i>k</i><sub>i</sub></td><td>2</td></tr><tr><td colspan="4"><b>LOS Guidance</b></td></tr><tr><td><i>k</i><sub>u</sub></td><td>60</td><td><i>k</i><sub>i,u</sub></td><td>3</td></tr><tr><td><i>k</i><sub>d,u</sub></td><td>1</td><td><i>k</i><sub>r</sub></td><td>20</td></tr><tr><td><i>k</i><sub>i,r</sub></td><td>1</td><td><i>k</i><sub>d,r</sub></td><td>10</td></tr><tr><td><i>K</i><sub>p</sub></td><td>20</td><td><i>K</i><sub>i</sub></td><td>5</td></tr><tr><td><i>K</i><sub>d</sub></td><td>8</td><td></td><td></td></tr><tr><td colspan="4"><b>Sliding Mode Controller</b></td></tr><tr><td><i>l</i><sub>x</sub></td><td>1</td><td><i>l</i><sub>y</sub></td><td>1</td></tr><tr><td><i>k</i><sub>x</sub></td><td>1</td><td><i>k</i><sub>y</sub></td><td>1</td></tr><tr><td>λ<sub>1</sub></td><td>5</td><td>λ<sub>2</sub></td><td>5</td></tr><tr><td>λ<sub>3</sub></td><td>1</td><td><i>K</i><sub>u</sub></td><td>2</td></tr><tr><td><i>K</i><sub>r</sub></td><td>2</td><td><i>W</i><sub>u</sub></td><td>1</td></tr><tr><td><i>W</i><sub>r</sub></td><td>1</td><td></td><td></td></tr></tbody></table>
## 3.5 Path-Following application of the $\mathcal{H}$ matrix controller

In this section, the $\mathcal{H}$ matrix controller is applied to a path-following problem. The Remus100 AUV has been used for this illustration and the task is a mock-up of a typical mission. It is made up of a series of waypoints joined by straight routes.

As described briefly in section 1.4.2.2, in the path-following scenario, the vehicle is constrained to the path between the waypoints. A virtual particle is simulated on the path, evolving according to the behavior of the vehicle so that it can never leave the actual particle representing the vehicle behind. The path-following approach used in this example is adapted from [20] and similar work can be found for instance in [111].

In path following, the geometric and dynamic tasks are tackled independently. In this example however, the solution given by the $\mathcal{H}$ controller is demonstrated to solve the dynamic task and provide a general solution to the geometric task at the same time.

### 3.5.1 The Remus100 AUV

The Remus100 (Remote Environmental Monitoring UnitS) shown on figure 3.14 is a commercial AUV designed by the Oceanographic System Lab. of the Woods Hole Oceanographic Institution and used in applications of hydrographic surveys, scientific sampling or pipeline inspections. It has been used in many research and industry applications and has a very well established model.

The Remus100 is equipped with a fixed rear thrusters, a rudder and a stern plane. The actuation configuration of the vehicle generates an independent surge force and pitch and yaw moments depending on the relative velocity of the vehicle and the surrounding water. Following the remark made on coupled DOF in section 5.1, the sway and heave forces generated by the stern control surfaces are neglected in the controller calculations.

<figure><img src="image_30.png" /><figcaption>Figure 3.14 – Remus100 (Image from OSL WHOI)</figcaption></figure>
A simplified allocation model has been used to calculate the control surfaces deflection angles and the necessary propeller speed.

### 3.5.2 $\mathcal{H}$ matrix controller for Remus100

The $\mathcal{H}$ matrix controller for the Remus100 vehicle is introduced in this section. The task is focused only on the position of the vehicle, the attitude of the vehicle is stabilized by hydrodynamic effects and hydrostatic restoring moments acting on the hull and control surfaces. Using an off-centered tracking point also enhances the natural attitude stability of the vehicle. As evoked before, the vehicle is actuated in surge, pitch and yaw.

Recalling the DOF representation introduced in section 3.2.2, the actuated DOF of the vehicle at the center of the mobile frame are represented as:

$$h_{o_B} = \begin{bmatrix} 1 & 0 & 0 & 0 & 1 & 1 \end{bmatrix}^{\top} \qquad (3.50)$$

To allow compensation of the lack of actuation in sway and heave, the tracking point is placed at the nose of the vehicle, $E = [\epsilon_x, 0, 0]^\top$ with $\epsilon_x$ approximately equal to half the hull length of the AUV. Having the tracking point at the front of the vehicle also increases the natural pitch and yaw stability. The controlled DOF at the tracking point are represented as:

$$h_E = \begin{bmatrix} 1 & 1 & 1 & 0 & 0 & 0 \end{bmatrix}^{\top} \qquad (3.51)$$

The Remus100 vehicle associated with the position task of this example match with the example given in section 3.2.3. Using algorithm 1 with $h_{o_B}$ and $h_E$ defined in equation (3.50) and (3.51) respectively, the $\mathcal{H}$ matrix is calculated as:

$$\mathcal{H} = \begin{bmatrix} 1 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & -\frac{1}{\epsilon_x} & 0 & 1 & 0 \\ 0 & \frac{1}{\epsilon_x} & 0 & 0 & 0 & 1 \end{bmatrix} \qquad (3.52)$$
Then, using the equations introduced in chapter 3, the $\mathcal{H}$ matrix controller is built as a cascaded model-based linearizing controller:

$$ \nu_c = \mathcal{H}T^{-1}J(\eta)^{-1}\lambda(\eta_E, \dot{\eta}_E, \alpha) \qquad (3.53a) $$

$$ \tau_c = M(\dot{\nu}_c + Ke_\nu) + C(\nu)\nu + D(\nu)\nu + g(\eta) \qquad (3.53b) $$

where $\alpha$ is an auxiliary variable to be defined later and $\lambda(\eta_E, \dot{\eta}_E, \alpha)$ is a control law to be determined. Because of the path following scenario, the control law will be a function of the current state of the vehicle and of the current state of the virtual path particle contained in $\alpha$ which is not known a priori but will be used as a control parameter and defined in the following. The non linear terms of the model are canceled using feedback linearization. The matrix $K$ is a conventional positive definite proportional gain matrix and $e_\nu = \nu_c - \nu$.

A complete proof of the stability of the $\mathcal{H}$-based controller can be found in chapter 3. Nevertheless, the cascaded structure of the system allows a rapid convergence demonstration. The feedback linearizing structure of the dynamic part of the controller equation (3.53b) gives the closed-loop system:

$$ \dot{\nu} = \dot{\nu}_c + Ke_\nu \quad \Rightarrow \quad \nu \to \nu_c \qquad (3.54) $$

**Remark 3.3** Here, as in section 3.3.3, the stability demonstration is given on the full six DOF state space. The stability of the non actuated DOF is assumed considering the great number of applications of the Remus100 vehicle where possible instability of the non actuated DOF due to coupling effects are not taken into account. A more rigorous demonstration should demonstrate the stability of the actuated DOF first, then show that the disturbance generated on the non actuated DOF does not make them unstable.

Then, providing that the inner loop is faster than the outer loop, applying equation (3.53a) to equation (1.3) gives the closed-loop system:

$$ \dot{\eta}_E = J(\eta)T\mathcal{H}T^{-1}J(\eta)^{-1}\lambda(\eta_E, \dot{\eta}_E, \alpha) \qquad (3.55) $$
with:

$$ T\mathcal{H}T^{-1} = \begin{bmatrix} 1 & 0 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & -\frac{1}{\varepsilon_x} & 0 & 0 & 0 \\ 0 & \frac{1}{\varepsilon_x} & 0 & 0 & 0 & 0 \end{bmatrix} \quad (3.56) $$

Breaking down the velocity vector into translation and rotation components $\dot{\eta}_E = [\dot{\eta}_{E_1}^\top, \dot{\eta}_{E_2}^\top]^\top$ and the control law in the same way, $\lambda = [\lambda_1^\top, \lambda_2^\top]^\top$, the closed-loop system (3.55) gives:

$$ \dot{\eta}_{E_1} \equiv \lambda_1 \quad (3.57) $$

The $\lambda(\eta_E, \dot{\eta}_E, \alpha)$ function must therefore be chosen to solve the path following problem described in the following.

### 3.5.3 The path-following problem

In this section, the path-following problem is described using the formalism of [20]. The position of the particle representing the tracking point of the vehicle is denoted $\mathbf{p}$ and the position of the virtual particle evolving on the path is denoted $\mathbf{p}_p$. The geometric path is parametrized by the scalar variable $\varpi$. It is defined as:

$$ P = \{ \mathbf{p} \in \mathbb{R}^3 \mid \mathbf{p} = \mathbf{p}_p(\varpi) \quad \forall \varpi \in \mathbb{R} \} \quad (3.58) $$

The geometric path is defined under the following assumptions motivated by the stability results described later in this section (see equation (3.70)):

A.1 The path is regular: $\varpi > 0, \dot{\varpi} > 0 \forall t > 0$.

A.2 The virtual particle is only moving forward on the path with the velocity vector $\mathbf{v}_p = [u_p, 0, 0]^\top$ expressed in the path frame with $u_p > 0$.
$$ \chi_p(\bar{\omega}) = \arctan \left( \frac{y'_p(\bar{\omega})}{x'_p(\bar{\omega})} \right) \qquad (3.59a) $$

$$ \nu_p(\bar{\omega}) = \arctan \left( \frac{-z'_p(\bar{\omega})}{\sqrt{x'_p(\bar{\omega})^2 + y'_p(\bar{\omega})^2}} \right) \qquad (3.59b) $$

where ′ is the derivative operator $\frac{d}{d\bar{\omega}}$. The two associated rotation matrices are given as:

$$ \mathbf{R}_{p,z}(\chi_p(\bar{\omega})) = \begin{bmatrix} \cos(\chi_p) & -\sin(\chi_p) & 0 \\ \sin(\chi_p) & \cos(\chi_p) & 0 \\ 0 & 0 & 1 \end{bmatrix} \qquad (3.60a) $$

$$ \mathbf{R}_{p,y}(\nu_p(\bar{\omega})) = \begin{bmatrix} \cos(\nu_p) & 0 & \sin(\nu_p) \\ 0 & 1 & 0 \\ -\sin(\nu_p) & 0 & \cos(\nu_p) \end{bmatrix} \qquad (3.60b) $$

The full rotation from the inertial frame to the path frame is hence defined as:

$$ \mathbf{R}_p(\bar{\omega}) = \mathbf{R}_{p,z}(\chi_p(\bar{\omega}))\mathbf{R}_{p,y}(\nu_p(\bar{\omega})) \qquad (3.61) $$

The following error between the virtual particle on the path and the tracking point of the vehicle is then defined in the path frame:

$$ \boldsymbol{\epsilon} = \mathbf{R}_p^\top (\mathbf{p} - \mathbf{p}_p(\bar{\omega})) \qquad (3.62) $$

Note that, in order to stick to the formalism of [20], the error convention used in this section is the opposite of the convention used elsewhere in this work. The error vector $\boldsymbol{\epsilon} = [x_e, y_e, z_e]^\top$ is broken down into the along-track error $x_e$, cross-track error $y_e$ and vertical-track error $z_e$. The path-following problem thus becomes the error minimizing problem of driving the error vector $\boldsymbol{\epsilon}$ to zero.

The path following problem is once again solved using Lyapunov theory. Consider the
<figure><img src="image_31.png" /><figcaption>Figure 3.15 – Path following problem in the horizontal plane. Blue : Path particle, Red : Vehicle</figcaption></figure>
Lyapunov function candidate $V_{\varepsilon}$:

$$V_{\varepsilon} = \frac{1}{2} \varepsilon^{\top} \varepsilon \qquad (3.63)$$

The first-order time derivative of the error vector expressed in the path frame is:

$$\begin{aligned}
\dot{\varepsilon} &= \dot{R}_p^{\top} (\mathbf{p} - \mathbf{p}_p) + R_p^{\top} (\dot{\mathbf{p}} - \dot{\mathbf{p}}_p) \\
&= S_p^{\top} R_p^{\top} (\mathbf{p} - \mathbf{p}_p) + R_p^{\top} (\dot{\mathbf{p}} - \dot{\mathbf{p}}_p) \\
&= S_p^{\top} \varepsilon + R_p^{\top} (\dot{\mathbf{p}} - \dot{\mathbf{p}}_p)
\end{aligned}
\qquad (3.64)$$

with the skew-symmetric matrix $S_p = -S_p^{\top}$:

$$S_p^{\top} = \begin{bmatrix}
0 & -\dot{\chi}_p \cos(\nu_p) & \dot{\nu}_p \\
\dot{\chi}_p \cos(\nu_p) & 0 & \dot{\chi}_p \sin(\nu_p) \\
-\dot{\nu}_p & -\dot{\chi}_p \sin(\nu_p) & 0
\end{bmatrix} \qquad (3.65)$$

The first-order time derivative of the Lyapunov candidate $V_{\varepsilon}$ becomes:

$$\begin{aligned}
\dot{V}_{\varepsilon} &= \varepsilon^{\top} (S_p^{\top} \varepsilon + R_p^{\top} (\dot{\mathbf{p}} - \dot{\mathbf{p}}_p)) \\
&= \varepsilon^{\top} R_p^{\top} (\dot{\mathbf{p}} - \dot{\mathbf{p}}_p)
\end{aligned}
\qquad (3.66)$$

At this point, the kinematic and dynamic parts of the task are considered to be solved. This means that the velocity of the tracking point $\dot{\mathbf{p}}$ is equal to a desired virtual velocity vector $\dot{\mathbf{p}}_{dv}$. Hence $\dot{V}_{\varepsilon}$ becomes:

$$\dot{V}_{\varepsilon} = \varepsilon^{\top} R_p^{\top} (\dot{\mathbf{p}}_{dv} - \dot{\mathbf{p}}_p) \qquad (3.67)$$

The desired velocity vector and the velocity of the virtual particle are then designed to respect the stability criterion $\dot{V}_{\varepsilon} \le 0$.

In [20], the desired virtual velocity vector $\dot{\mathbf{p}}_{dv}$ is constructed so as to introduce LOS guidance. Here, according to the results of section 3.5.2, the desired velocity vector can be chosen as the output of the position control law $\lambda_1$ where the parameter $\alpha$ is chosen as the position of the virtual particle $\mathbf{p}_p$. Using the $\mathcal{H}$ based controller, one gets $\dot{\mathbf{p}} = \dot{\eta}_{E_1} \equiv \lambda_1(\eta_{E}, \dot{\eta}_{E}, \mathbf{p}_p) = \dot{\mathbf{p}}_{dv}$.

In a first approximation, the control law $\lambda_1$ is chosen as a proportional controller
based on the following error ε and moved to the inertial frame using $R_p$ associated with a feedforward term:

$$ \lambda_1 = R_p v_d - \Lambda R_p \varepsilon \qquad (3.68) $$

with $\Lambda$ a diagonal gain matrix with equal coefficients on the diagonal $\Lambda = \lambda I_3$ with $\lambda > 0$. The vector $v_d = [U_d, 0, 0]^\top$ is the desired velocity of the vehicle expressed in the path frame where the desired surge speed $u_d$ is a control parameter defined in the following.

Then, because the virtual path particle is constrained to the path, its velocity vector in the inertial frame is defined as:

$$ \dot{p}_p = R_p v_p \qquad (3.69) $$

with $v_p = [u_p, 0, 0]^\top$ and $u_p$ the speed of the particle which is a control parameter, chosen later to ensure stability and move the virtual particle according to the behavior of the vehicle.

The time derivative of the Lyapunov function candidate becomes:

$$ \begin{aligned} \dot{V}_{\varepsilon} &= \varepsilon^{\top} R_p^{\top} (R_p v_d - \Lambda R_p \varepsilon - R_p v_p) \\ &= \varepsilon^{\top} (v_d - \Lambda \varepsilon - v_p) \\ &= -\lambda(x_e^2 + y_e^2 + z_e^2) + (u_d - u_p)x_e \end{aligned} \qquad (3.70) $$

Here, the speed of the virtual particle $u_p$ can be chosen to cancel the second term of the right hand member, $u_p = u_d$ and thus $\dot{V}_{\varepsilon} \le 0$ for all $\lambda > 0$.

The path-following problem can be updated to a *path-tracking* problem as described in [20]. In path-tracking, the speed of the vehicle is adapted to catch up with the particle. To do so, the desired speed of the vehicle is expressed as a function of the along-track error:

$$ u_d = u_p - k_x x_e \qquad (3.71) $$

with $k_x$ a gain parameter.

The time derivative of the Lyapunov function candidate becomes:

$$ \dot{V}_{\varepsilon} = -\lambda(x_e^2 + y_e^2 + z_e^2) - k_x x_e^2 \qquad (3.72) $$

In such a case, the equilibrium $\varepsilon = 0$ is stable for any $\lambda > 0$ and $k_x > 0$.

This last choice of $u_d$ is particularly interesting since it allows an operator to choose the behavior of the whole system (vehicle and path particle) by tuning only the particle speed
$u_p$ and the control parameters. Overall, the equilibrium $\varepsilon = \mathbf{o}$ is shown to be globally exponentially stable.

The path tracking scheme can easily be adapted to other scenarios. As an example, in a robotic fleet, the path particle can represent a primary vehicle following a predefined task. Other secondary vehicles of the fleet are tracking the primary one for instance going back and forth to the surface for communication. It is then very important that all the secondary vehicles adapt their speed to the primary one.

Another similar solution to equation (3.71) is to choose the desired speed of the vehicle and adapt the particle speed so that the vehicle is not left behind. This is a different approach where the desired speed of the ship is prioritized. It implies that no time constraint can be added to the task as the behavior of the vehicle may vary between the waypoints. In this case, the particle speed is:

$$u_p = u_d + k_x x_e \qquad (3.73)$$

The derivative of the Lyapunov function is obviously similar to equation (3.72) using this solution. One major difference is that the path particle speed $u_p$ may not meet with assumptions A.1 and A.2. This approach also creates the stable equilibrium $\varepsilon = \mathbf{o}$ for any $\lambda > 0$ and $k_x > 0$. Yet, it does not ensures that the path is regularly defined. The main advantage is that it allows a pilot to choose the desired speed of the craft and the particle adapts.

### 3.5.4 Simulation results

The controller developed in Sections 3.5.2 and 3.5.3 is applied in simulation to the model of the Remus100. These simulations were performed on a different simulator based on the MSS toolbox [70]. Experiments are conducted with a simulated measurement noise of standard deviation 0.1 m for the position measurements and 0.01rad for the orientations. The path is defined as straight lines in 3-D joining waypoints. No additional constraint is added to the task and the vehicle is naturally stable in roll and pitch. Note that the shape and actuation configuration of the Remus100 allows neglecting the sway and heave forces generated by the actuators in the controller. The vehicle is considered actuated in surge, pitch and yaw.

A simplified allocation procedure is used to calculate the deflection angles of the rudder
and stern plane of the vehicle $\delta_r$ and $\delta_s$:

$$ \delta_r = \frac{2N_c}{\frac{L}{2}\rho U_h^2 A_s C_{\delta_s}} \qquad (3.74a) $$

$$ \delta_s = \frac{2M_c}{\frac{L}{2}\rho U_v^2 A_r C_{\delta_r}} \qquad (3.74b) $$

where $M_c$ and $N_c$ are the fifth and sixth components of the control vector $\tau_c$, $\rho$ is the local water density, $L$ is the length of the hull of the vehicle, $U_h$ and $U_v$ are the $\mathbf{y}_B$ axis and $\mathbf{z}_B$ axis relative speeds of the vehicle and $A_s$, $A_r$, $C_{\delta_s}$ and $C_{\delta_r}$ are known physical parameters of the control surfaces.

The target switches from one waypoint to the next as soon as the tracking point of the vehicle arrives in a circle of 0.1 m radius around the current target.

### 3.5.4.1 Without external disturbance

With no external current, the controller gives very good following results. The position of the tracking point and virtual particle are shown on figure 3.16. Figure 3.17 shows that the vehicle lines up with the path between the turns and stays tangent to the $x$ axis of the path frame at all times. The pitch and yaw angles follow the desired elevation and azimuth angles (respectively) of the path.

### 3.5.4.2 With sea current

In this experiment, the vehicle is exposed to a 0.5 m s⁻¹ sea current aligned with the $\mathbf{y}_0$ axis. Figure 3.18 shows that the path following performances are not decreased in presence of external disturbance and measurement noise. Nonetheless, figure 3.19 shows that the yaw equilibrium taken by the vehicle between the turns is different from the azimuth angle of the path. With sideways current, the vehicle cannot stay tangent to the path but finds a different equilibrium. Yet, figure 3.20 shows that the error in the path frame is canceled even with external disturbance. Even though robustness to external disturbance has not been demonstrated on this example, the cascade structure of the controller, integral action and use of a tracking point hint towards robustness to bounded disturbances.
<figure><img src="image_32.png" /><figcaption>Figure 3.16 – Position of the tracking point (Blue) of the vehicle with no current.
Dashed Red: Position of virtual particle, Dashed black: Waypoints</figcaption></figure>
<figure><img src="image_33.png" /><figcaption>Figure 3.17 – Attitude of the Remus100 vehicle (Blue) compared to the azimuth and elevation angle of the path (Dashed black)</figcaption></figure>
<figure><img src="image_34.png" /><figcaption>Figure 3.18 – Position of the tracking point (Blue) of the vehicle with current. Dashed Red: Position of virtual particle, Dashed black: Waypoints</figcaption></figure>
<figure><img src="image_35.png" /><figcaption>Figure 3.19 – Attitude of the Remus100 vehicle (Blue) with sea current compared to the azimuth and elevation angle of the path (Dashed black)</figcaption></figure>
<figure><img src="image_36.png" /><figcaption>Figure 3.20 – Path following error in the path frame.</figcaption></figure>
3.6 Partial conclusions on *H* based control

In this chapter, a novel control method for underactuated marine craft has been pre-
sented. This controller relies on a guidance stage based on the new Handy matrix H. The
matrix creates compensation behaviors between the different DOF of the system. Notably,
it allows exploiting actuated rotations of the vehicle to compensate for the lack of some
translations as it is usually the case with marine vehicles. The method has been shown
to perform as well as a Line Of Sight guidance principle and a Sliding Mode Controller,
two reference control methods in the literature.

The convergence calculations and simulation results show that the errors on the con-
trolled DOF globally exponentially converge to zero. The cascade structure of the system
makes it reliable and robust to external disturbances. The H matrix does not create any
pole or zero in the control law which entirely characterized by the gains of proportional
controller of the inner loop and the PI of the outer loop.

Nevertheless, this controller shows one of the drawbacks of any compensation control
methods. Because the attitude taken by the vehicle during the task is not completely
controlled, and notably in the examples above, the roll angle is not controlled, the vehicle
can stabilize in configurations in which actuation over one or several DOF of task is lost. It
is notably showing clearly on section 3.4.1. If the vehicle were to stabilize with a roll angle
of 90°, actuation over the rotation around the vertical axis of the inertial frame z₀ would
be lost and the vehicle would not be able to meet the task constraints. The equilibrium on
the DOF used for compensation, roll in the examples, is hardly predictable as it depends
on AUV dynamics and environmental factors. As a matter of facts, limitations can be set
on the DOF used for compensation to avoid singular attitudes with the counterpart of
reducing performance.

The kinematic guidance principle has also been applied to the path-following problem.
This problem and the Remus100 vehicle used for example are a very common underwater
robotics problem. The $\mathcal{H}$ matrix controller allows perfect path-following in this case even
with unmodeled disturbances and measurement noise. This path-following problem could
easily be extended to a leader-follower fleet problem where one vehicle as to follow another
one performing a specified mission.

Further studies on physical systems will be conducted to discriminate the different
compensation solutions offered by the method. Notably, an energetic study comparing
the total energy consumption of two strategies would allow choosing one optimal solution
over the other and help for the choice of a solution.
# FLATNESS-BASED CONTROL OF MARINE VEHICLES

## Contents

<table><tr><td>4.1</td><td>Introduction to flatness</td><td>150</td></tr><tr><td>4.2</td><td>Fully actuated case</td><td>154</td></tr><tr><td>4.2.1</td><td>Flat Output</td><td>154</td></tr><tr><td>4.2.2</td><td>Flatness of the system</td><td>155</td></tr><tr><td>4.2.3</td><td>Description of the trajectory</td><td>155</td></tr><tr><td>4.2.4</td><td>Flatness-based controller</td><td>156</td></tr><tr><td>4.3</td><td>Underactuated case</td><td>157</td></tr><tr><td>4.3.1</td><td>Model of the underactuated surface vessel</td><td>157</td></tr><tr><td>4.3.2</td><td>Flatness defects of the surface vessel</td><td>159</td></tr><tr><td>4.3.3</td><td>Special case: the Hovercraft system</td><td>160</td></tr><tr><td>4.4</td><td>iPID</td><td>169</td></tr><tr><td>4.4.1</td><td>Implementation of the iPID</td><td>169</td></tr><tr><td>4.5</td><td>Flatness simulations</td><td>172</td></tr><tr><td>4.5.1</td><td>Fully-actuated AUV</td><td>172</td></tr><tr><td>4.5.2</td><td>Underactuated surface vessel</td><td>180</td></tr><tr><td>4.6</td><td>Conclusion</td><td>192</td></tr></table>

In this chapter, flatness-based controllers are derived for both the fully actuated marine craft and the underactuated surface vessel. Flatness-based control has been briefly introduced in section 2.3. It has shown very good results on both fully actuated and under actuated systems as the mechanical crane or the car with trailers. The main quality of flatness-based control is its robustness to both model approximations and disturbances making it a strong contestant for the control of marine craft.
As shown in this chapter, flatness-based controller relies on the inherent properties of a physical system. This chapter demonstrates that the flat character of a system can also depend on the actual numerical values of some sets of model parameters. This last property is exploited in this chapter to derive a flatness-based controller for systems which are not naturally flat.

To further increase the robustness of flatness-based control on underactuated vehicles, it is associated with Intelligent PID section 2.5.1. Here, the iPID is used to allow applying the flatness based controller calculated on a special case of surface vessels, the Hovercraft, to any surface vessel. The iPID also compensates for one of the weaknesses of flatness-based control, its unstable behavior away from the trajectory.

## 4.1 Introduction to flatness based control

In this section, the concept of flatness-based control is introduced with a theoretical example derived from [90] and studied in details in [84]. It is a simple MIMO system given as:

$$ \dot{x}_1 = u_1 \qquad (4.1a) $$

$$ \dot{x}_2 = x_3 + x_2 u_1 \qquad (4.1b) $$

$$ \dot{x}_3 = u_2 \qquad (4.1c) $$

where $u_1$ and $u_2$ are the two control inputs and $x[x_1, x_2, x_3]^T$ is the state.

As evoked before, (differential) flatness, is an inherent property of a class of systems, notably useful for the design of controllers and often associated with feedforward linearization and extended PID control [84]. First, the plant must be shown flat meaning that all variables of the system can be expressed as function of a particular set of variables, the flat outputs, and their derivatives. The flat output is a set of independent differential functions of the system variables which can be used to express all other variables of the system [139]. The flat output must have as many components as the number of inputs of the plant. A flat system can have several flat outputs.

In this example, the system has two inputs. The flat output must therefore have two independent components. On physical systems, the flat output is often chosen as a set of variables of interest for the system. Working with mobile robots for instance, the position and orientation of the vehicle are good candidates for the flat output. Also, the task is
likely to be defined in terms of desired values of the flat output making a strong argument in the choice of the flat output when several solutions exist.

To show flatness of a system, flat output candidates are tested. The first flat output candidate denoted $z$ is chosen as: $z = [x_1 x_2]^\top$ in this example. Showing that this system is flat means demonstrating that all other variables of the system can be expressed as functions of the flat output and its derivatives without solving any differential equation.

The three variables of the system are therefore expressed as functions of $x_1$ and $x_2$:

$$u_1 = \dot{x}_1 \qquad (4.2a)$$

$$x_3 = \dot{x}_2 - x_2 \dot{x}_1 \qquad (4.2b)$$

$$u_2 = \ddot{x}_2 - \dot{x}_2 \dot{x}_1 - x_2 \ddot{x}_1 \qquad (4.2c)$$

These formulations of the intermediary variable $x_3$ and the two inputs $u_1$ and $u_2$ show that the system is flat. The flat output candidate $z$ is indeed a flat output of this system. Equations like (4.2) will be referred to as *equations of flatness* or *flatness equations* in the following. They are notably used as a base in the control calculations.

In this example, the pair $x_1, x_2$ is the most natural flat output. Whereas some systems may accept several flat outputs, here equation (4.1b) implies that any candidate containing $x_3$ would require integrating a differential equation to show flatness. For instance, if $z' = [x_1 x_3]^\top$ were to be chosen, $x_2$ could not be expressed as an algebraic function of the flat output but instead as a differential expression $\dot{x}_2 = x_3 + x_2 \dot{x}_1$ preventing demonstration of flatness. This is referred to as a flatness defect (see [53] for more details).

One important property of flat systems, stated for instance in [38], is that all flat systems can be represented using the *Brunovský state* [23]. The Brunovský representation of system is a minimal representation where a succession of change of variables allow representing the system as a set of integrator chains. The Brunovský state representation is particularly useful for the design of the control law in flatness-based control as it allows turning complex non-linear systems into simpler integrator chains. Note that [38] introduces a *controller form* based on the Brunovský state. A simple feedback takes the system from the controller form to the Brunovský form.
Quoting [84] again, the Brunovský state for this system is:

$$ \xi_{1,1} = x_1 \qquad (4.3a) $$

$$ \xi_{2,1} = x_2 \qquad (4.3b) $$

$$ \xi_{2,2} = \dot{x}_2 \qquad (4.3c) $$

Then, the system can be rewritten as:

$$ \dot{\xi}_{1,1} = u_1 \qquad (4.4a) $$

$$ \dot{\xi}_{2,1} = \xi_{2,2} \qquad (4.4b) $$

$$ \dot{\xi}_{2,2} = \xi_{2,2}u_1 + \xi_{2,1}\dot{u}_1 + u_2 \qquad (4.4c) $$

At this point, introducing the new inputs $v_1 = u_1$ and $v_2 = \xi_{2,2}u_1 + \xi_{2,1}\dot{u}_1 + u_2$ allows writing equation (4.4) as two simple chains of integrators, which facilitates the calculations of the controller and leads to a unique solution for the original inputs:

$$ u_1 = v_1 \qquad (4.5a) $$

$$ u_2 = v_2 - \xi_{2,2}\dot{\xi}_{1,1} - \xi_{2,1}\ddot{\xi}_{1,1} \qquad (4.5b) $$

The new system reads:

$$ \dot{\xi}_{1,1} = v_1 \qquad (4.6a) $$

$$ \dot{\xi}_{2,1} = \xi_{2,2} \qquad (4.6b) $$

$$ \dot{\xi}_{2,2} = v_2 \qquad (4.6c) $$

The control inputs for this new simplified system are obviously calculated in a feedforward and extended PID fashion as:

$$ v_1^c = \dot{\xi}_{1,1}^* + \text{PI}(e_{1,1}) \qquad (4.7a) $$

$$ v_2^c = \dot{\xi}_{2,2}^* + \text{PID}(e_{2,1}) \qquad (4.7b) $$

In equation (4.7), the upperscript * denotes desired values and the functions $\text{PI}(\xi_1)$ and $\text{PID}(\xi_2)$ represent usual PI and PID controllers calculated on the errors on $\xi_{1,1}$ and $\xi_{2,1}$ respectively.
Using equation (4.5), the original control inputs are calculated as:

$$u_1^c = \dot{\xi}_{1,1}^* + \mathrm{PI}(\xi_{1,1}) \qquad (4.8a)$$

$$u_2^c = \dot{\xi}_{2,2}^* + \mathrm{PID}(\xi_{2,1}) - \xi_{2,2}^* \dot{\xi}_{1,1}^* - \xi_{2,1}^* \ddot{\xi}_{1,1}^* \qquad (4.8b)$$

where all variables of lesser order have been replaced by their desired values issued from the trajectory. Because all the variables of the system have been defined as functions of the flat output, the desired values for each of them can be calculated for the complete trajectory. Using the desired values of the system variables in the controller gives flatness-based control its robustness but also limits the range of application of flatness-based control to the vicinity of the trajectory. This highlights that the trajectory of the flat output must be sufficiently smooth and continuously defined up to a high enough order, here to the second order, to allow proper feedforward linearization.

In facts, a faster but less rigorous method —working on most flat systems— can be used to build the controller without writing the Brunovský state. The idea is to seek, in each flatness equation of inputs (in this example equation (4.2a) and (4.2c)), a *favored* or *preferential* flat output component and replace it with an extended PID controller and a feedforward term of the higher order. The rest of the variables are replaced by their desired values in a feedforward linearizing manner. The preferential output component generally appears with a higher derivation order in the input flatness equation. Here, the choice is obvious in equation (4.2): the first input $u_1$ only depends on the first order derivative of the first input component $\dot{x}_1$ so $u_1$ can only be used to control $x_1$. For $u_2$, both $x_1$ and $x_2$ appear at the second order in equation (4.2c) but $x_1$ has already been chosen in $u_1$ so, to avoid redundancies, only $x_2$ can be used. Following this rule, the two control inputs are:

$$u_1^c = \dot{x}_1^* + \mathrm{PI}(e_1) \qquad (4.9a)$$

$$u_2^c = \ddot{x}_2^* + \mathrm{PID}(e_2) - \dot{x}_2^* \dot{x}_1^* - x_2^* \ddot{x}_1^* \qquad (4.9b)$$

Note that, because $x_1$ appears at the first order in $u_1$ a PI controller is sufficient while in $u_2$, because $x_2$ appears at the second order, a PID controller is better suited [84, 85]. In this example, the resulting expression (4.9) is equivalent to equation (4.8).

This method does not always allow designing the controller for flat systems and would not work on a system where no flat output component appear preferential in any of the input flatness equations (see for instance the hovercraft system in section 4.3.3).
## 4.2 Flatness-based control in the fully actuated case

In this section, flatness-based control is applied to the example of a fully-actuated underwater vehicle (see for example the vehicle introduced in section 1.2.1.1). On fully-actuated plants, the end result of flatness-based control is naturally close to feedforward linearizing control but the approach is different. One of the few applications of flatness to an underwater vehicle is found in [137] but a variation of the method is used here.

The model of the fully-actuated underwater vehicle is recalled here. It is expressed in the inertial frame [8] as it is the most self-explanatory representation for flatness-based control but the same results are obtained with the dynamic model expressed in the mobile frame as in equation (1.5).

$$ \tau = \tilde{M}(\eta)\ddot{\eta} + \tilde{C}(\eta, \dot{\eta})\dot{\eta} + \tilde{D}(\eta, \dot{\eta})\dot{\dot{\eta}} + g(\eta) \quad (4.10a) $$

$$ \tilde{M}(\eta) = M J(\eta)^{-1} \quad (4.10b) $$

$$ \tilde{C}(\eta, \dot{\eta}) = M \dot{J}(\eta, \dot{\eta})^{-1} + C(\nu) J(\eta)^{-1} \quad (4.10c) $$

$$ \tilde{D}(\eta, \dot{\eta}) = D(\nu) J(\eta)^{-1} \quad (4.10d) $$

where $\tilde{M}(\eta)$, $\tilde{C}(\eta, \dot{\eta})$ and $\tilde{D}(\eta, \dot{\eta})$ are the model matrices expressed in the inertial frame (see section 1.1.3 for details). In the following, the arguments of matrices $\tilde{M}$, $\tilde{C}$ and $\tilde{D}$ will be omitted for the sake of clarity and conciseness.

In this chapter, as in many of the examples of chapter 2, the vector of propulsive forces and moments $\tau$ is considered to be the input of the system. In facts, when working with fixed thrusters, the real input of the system is the vector of thrusts $u$. Yet, the relationship $\tau = Bu$ being static, using $\tau$ as an input is acceptable with fixed thrusters. With reconfigurable thrusters or control surfaces, the relation between $\tau$ and the real inputs of the system (like the rudder angle or the reconfiguration angle of a vector thruster) may prevent showing flatness of the complete system as it adds more non linearities in the model.

### 4.2.1 Flat Output

From a broader point of view, the flat output can be seen as a set of variables of interest of the system whom main property is that it is naturally central to the system in the sense that all other variables of the system can be expressed as functions of this set
and its time derivatives. The relationship between the flat output and the other variables is inherent to the system. In the fully actuated case the choice of the flat output candidate comes naturally as $z = \eta$.

A different choice like $z = \nu$ could work but would be more complex to use. It would for instance be harder to have order zero position terms in the controller or to add an integral effect on the position and orientation.

### 4.2.2 Flatness of the system

In the fully actuated example shown in equation (4.10), the only variables other than the flat output candidate $z = \eta$ are the input vector $\tau$ and the velocity vector in the mobile frame $\nu$. The dynamic model equation (4.10) of the example and the kinematic model equation (1.1) give naturally:

$$ \nu = J(\eta)^{-1} \dot{\eta} \qquad (4.11a) $$

$$ \tau = \tilde{M}\ddot{\eta} + \tilde{C}\dot{\eta} + \tilde{D}\dot{\eta} + g(\eta) \qquad (4.11b) $$

The fully actuated underwater vehicle is then flat with the flat output $z = \eta$. In this case, the equations of the model directly give the equations of flatness but it will not always be the case and notably not with underactuated systems.

### 4.2.3 Description of the trajectory

The task trajectory is usually defined in terms of desired values of the flat output. It must be sufficiently smooth meaning that it must be defined and differentiable to a sufficiently high order to match the order of derivation of the flat output needed to defined all variables of the system.

One methodology to define a suitable trajectory to flatness control is to start with a set of waypoints. Each waypoint imposes a desired position, velocity and acceleration on all six DOF of the flat output as well as a time constraint. The order of constraint of the waypoints must match the order of the system. Here in the case of the AUV, the trajectory must be defined up to the second order. Then, smooth paths are defined between the waypoints using, for instance, a piece-wise polynomial trajectory generator. It is mandatory to keep continuity around the waypoints between two consecutive pieces of path up to the highest order of the system.
Once the desired values of the flat output are properly defined, nominal values of the other variables of the system can be calculated using the flatness equations. Notably, a nominal input $\tau^*$ is calculated:

$$ \nu^* = J(\eta^*)^{-1}\dot{\eta}^* \qquad (4.12a) $$

$$ \tau^* = \tilde{M}(\eta^*)\ddot{\eta}^* + \tilde{C}(\eta^*, \dot{\eta}^*)\dot{\eta}^* + \tilde{D}(\eta^*, \dot{\eta}^*)\dot{\eta}^* + g(\eta^*) \qquad (4.12b) $$

### 4.2.4 Flatness-based controller

As briefly evoked in section 2.2.1, flatness-based controllers in the fully-actuated case often resemble a sort of feedforward linearizing controller. In facts, the nominal input defined with respect to the flat output equation (4.12b) provides feedforward terms that are used in the flatness-based controller. The closed loop system obtained with a flatness-based controller is nonlinear but the nonlinearities cancel out in the vicinity of the trajectory. Flatness-based controllers are therefore particularly suited for applications where the system is assured to stay in the neighborhood of the desired trajectory. As shown in [85], the remaining non-linear terms of the closed-loop system can behave favorably for the convergence and stability of the system.

The rigorous method to designing the flatness-based controller of a system starts with writing the model in the Brunovský state as shown in section 4.1. Yet, writing the Brunovský state representation for such a system can be quite strenuous as it requires separating and decoupling all six DOF of the flat output. Instead, the other method is used in this example. In facts, because the flat output and the input of the system are defined in two different frames related by the transformation matrix $J(\eta)$, no favored output component arise in any of the input flatness equations. The matrix $\tilde{M}(\eta)$ is non-diagonal for any vector $\eta$ thus several components of the flat output appear at the second order on all the input equations. Yet, provided that the $\tilde{M}$ matrix is of plain rank at all times, the vector formulation equation (4.11b) can be used and the complete $\ddot{\eta}$ vector replaced with the feedforward term and the controller. This condition on $\tilde{M}$ ensures that, for any configuration of the vehicle, no output component will be left uncontrolled during the application and no redundancy can appear.

The controller is designed directly on the vector flatness formulation equation (4.11b) replacing the second order instance of the complete flat output $\ddot{\eta}$ by a vector of six decoupled PID controllers, one per flat output, and a feedforward term $\ddot{\eta}^*$. The other
variables are substituted with their desired values. The controller reads:

$$ \tau_c = \tilde{M}(\eta^*)(\ddot{\eta}^* + \text{PID}(e_\eta)) + \tilde{C}(\eta^*, \nu^*)\dot{\eta}^* + \tilde{D}(\eta^*, \nu^*)\dot{\eta}^* + g(\eta^*) \quad (4.13) $$

In this equation, PID(e_η) is a vector-PID controller calculated with the complete error vector e_η. The gain matrices are diagonal. It is equivalent to having six independent PID controllers, one for each component of the flat output.

Applying this controller to the system (4.10) leads to the following closed-loop system:

$$ \begin{aligned} \tilde{M}(\eta^*)(\ddot{\eta}^* + \text{PID}(e_\eta)) - \tilde{M}(\eta)\ddot{\eta} + \tilde{C}(\eta^*, \nu^*)\dot{\eta}^* - \tilde{C}(\eta, \nu)\dot{\eta} \\ + \tilde{D}(\eta^*, \nu^*)\dot{\eta}^* - \tilde{D}(\eta, \nu)\dot{\eta} + g(\eta^*) - g(\eta) = 0 \quad (4.14) \end{aligned} $$

The resulting closed-loop system is nonlinear because of the feedforward terms used for linearization. Yet, in the neighborhood of the trajectory, the non linearities cancel out and, as demonstrated in [85], the non linearities of the closed-loop system can behave favorably. Notably, when the model parameters are approximated in the control calculations, the remaining non-linear terms can add robustness to the controller. The performances of this flatness-based controller are detailed and compared with other controllers in section 4.5.1.

## 4.3 Flatness-based control in the underactuated case

As seen with several examples throughout this work (see chapter 2) a large number of marine vehicles are underactuated or ill-actuated. This is notably the case of most surface ships. While flatness-based control is not typically associated to underactuated vehicles, some examples outside of the marine context show good control performances on underactuated systems. Flatness-based control has notably made a name for itself solving the problem of controlling a car with trailers [53]. Also, the natural robustness to external disturbance and model approximations of flatness-based control makes it an ideal candidate for marine applications.

### 4.3.1 Model of the underactuated surface vessel

The underactuated surface vessel has been used as an example in several studies on flatness-based control [154, 135]. In [154], two different models are presented for the surface ship giving similar results. In this section, the model issued from [63] for surface vessels
is used. Reminding the equations introduced in chapter 1 the model matrices are given for the surface vessel considering that the body-fixed frame is centered on the center of gravity as:

$$ M = \begin{bmatrix} m - X_{\dot{u}} & 0 & 0 \\ 0 & m - Y_{\dot{v}} & -Y_{\dot{r}} \\ 0 & -N_{\dot{v}} & I_0 - N_{\dot{r}} \end{bmatrix} \qquad (4.15a) $$

$$ C(\nu) = \begin{bmatrix} 0 & 0 & -(m - Y_{\dot{v}})v + Y_{\dot{r}}r \\ 0 & 0 & (m - X_{\dot{u}})u \\ (m - Y_{\dot{v}})v - Y_{\dot{r}}r & -(m - X_{\dot{u}})u & 0 \end{bmatrix} \qquad (4.15b) $$

$$ D(\nu) = - \begin{bmatrix} X_u & 0 & 0 \\ 0 & Y_v & Y_r \\ 0 & N_v & N_r \end{bmatrix} \qquad (4.15c) $$

Hence, the complete model of a surface vehicle actuated in surge and yaw (neglecting possible coupled sway actuation force) is given in component form as:

$$ \dot{x} = u \cos \psi - v \sin \psi \qquad (4.16a) $$

$$ \dot{y} = u \sin \psi + v \cos \psi \qquad (4.16b) $$

$$ \dot{\psi} = r \qquad (4.16c) $$

$$ \dot{u} = \frac{1}{m - X_{\dot{u}}} (\tau_u + (m - Y_{\dot{v}})vr - Y_{\dot{r}}r^2 + X_u u) \qquad (4.16d) $$

$$ \dot{v} = \frac{I_0 - N_{\dot{r}}}{(m - Y_{\dot{v}})(I_0 - N_{\dot{r}}) - Y_{\dot{r}}^2} (Y_{\dot{r}}\dot{r} - (m - X_{\dot{u}})ur + Y_v v + Y_r r) \\ - \frac{Y_{\dot{r}}}{(m - Y_{\dot{v}})(I_0 - N_{\dot{r}}) - Y_{\dot{r}}^2} (\tau_r + N_{\dot{v}}\dot{v} - (X_{\dot{u}} - Y_{\dot{v}})uv + Y_{\dot{r}}ur + N_v v + N_r r) \qquad (4.16e) $$

$$ \dot{r} = -\frac{Y_{\dot{r}}}{(m - Y_{\dot{v}})(I_0 - N_{\dot{r}}) - Y_{\dot{r}}^2} (Y_{\dot{r}}\dot{r} - (m - X_{\dot{u}})ur + Y_v v + Y_r r) \\ + \frac{m - Y_{\dot{v}}}{(m - Y_{\dot{v}})(I_0 - N_{\dot{r}}) - Y_{\dot{r}}^2} (\tau_r + N_{\dot{v}}\dot{v} - (X_{\dot{u}} - Y_{\dot{v}})uv + Y_{\dot{r}}ur + N_v v + N_r r) \qquad (4.16f) $$
Equation (4.16) gives the kinematics and dynamics of an underactuated ship in the horizontal plane using the usual approximations of shape, added masses and damping relative to surface vessels. The model is expressed at the center of the mobile frame which is usually chosen at the center of gravity of the vehicle in surface applications. The two inputs of this system are $\tau_u$ and $\tau_r$, respectively a surge force and a yaw moment generated by either two fixed thrusters, a single vector thruster or a thruster associated with a control surface (in the two last cases, sway disturbances are neglected).

The non diagonal shape of the added mass and damping matrices create several coupled acceleration and speed terms in equation (4.16). Notably, a quadratic yaw speed term $Y_{\dot{v}}r^2$ appears in the surge dynamics equation (4.16d) and a yaw acceleration term $Y_{\dot{r}}\dot{r}$ appears in the sway dynamics equation (4.16e). These terms are often neglected as they will be in what follows.

### 4.3.2 Flatness defects of the surface vessel

Considering that most applications of the autonomous surface vehicle are setpoint regulation or path tracking tasks, the sensible choice for the flat output candidate would be the vector of position in the horizontal plane $z = [x \ y]^T$. Because the system has only two independent inputs, the flat output must have exactly two components. It is therefore impossible to consider the two position coordinates as well as the orientation $\psi$ as the flat output.

As stated in both [135] and [154], the general model of the surface vessel presented in equation (4.16) is not flat. Trying to express all systems variables as functions of $z$ and its time derivatives, one quickly realizes that no algebraic expression of the $\psi$ angle can be found. A differential expression is mandatory. Using the first time derivative of equation (4.16a) and equation (4.16b) and trigonometric combinations, the following relation is established:

$$ \ddot{x} \sin \psi - \ddot{y} \cos \psi = u \dot{\psi} - \frac{1}{m - Y_{\dot{v}}} \left( -(m - X_{\dot{u}})u \dot{\psi} + Y_v v + Y_{\dot{r}} \ddot{\psi} + Y_r \dot{\psi} \right) \quad (4.17) $$

Equation (4.17) is a second order differential equation in yaw angle $\psi$. The differential equations preventing the flatness demonstration are referred to as *flatness defects* [53]. The defect of a non flat system is the number of differential equations obtained when trying to show flatness of a system. Here, the system as a defect equal to 1.
**Remark 4.1** *Studying equation (4.17) also allows finding the defects preventing flatness of the system. As said before, flatness is an inherent property of a physical system but it can depend on numerical values of some constant parameters of the plant. For a given system as equation (4.16), some sets of numerical parameters can cancel the defects of a system and allow showing flatness while another set will give a non-flat system<fnref n="1" />. Notably, looking at equation (4.17), it appears clearly that the off-diagonal terms of the added mass and damping matrices $Y_f$ and $Y_r$ bring derivatives of the yaw angle in equation (4.16d) and equation (4.16e). Choosing to neglect these terms or working with systems where they are naturally null would modify the flatness property of the system. This is the case of the Hovercraft system studied in the following.*

Note that [154] presents a simplified version of the SV model as a *Liouvillian* system which is one of the *almost* flat properties of a system that can be used to calculate controllers. Here, this more complete expression of the plant cannot be shown to be Liouvillian.

### 4.3.3 Special case: the Hovercraft system

The hovercraft system is a typical case study of mobile robots. It is described as an underactuated surface vehicle controlled in surge and yaw like the surface vessel studied in the previous section but with the distinctive feature of being of circular shape with homogeneous mass distribution. This property of the hovercraft system translates in the added mass and damping matrices. The coupling terms between sway and yaw are canceled in both matrices: $Y_f = N_v = 0$ and $Y_r = N_v = 0$. Also, the circular shape of the vehicle implies that the hydrodynamic effects of damping and added mass on the surge and sway

<footnote n="1">1. To our best knowledge, this fact only appears implicitly in previous works about differential flatness.</footnote>
<figure><img src="image_37.png" /><figcaption>Figure 4.1 – Simplified representation of the hovercraft.</figcaption></figure>

axes are the same, giving $X_{\dot{u}} = Y_{\dot{v}}$ and $^2 X_u = Y_v$. The matrices of (4.15) become:

$$ M = \begin{bmatrix} m - X_{\dot{u}} & 0 & 0 \\ 0 & m - X_{\dot{u}} & 0 \\ 0 & 0 & I_0 - N_{\dot{r}} \end{bmatrix} \qquad (4.18a) $$

$$ C(\nu) = \begin{bmatrix} 0 & 0 & -(m - X_{\dot{u}})v \\ 0 & 0 & (m - X_{\dot{u}})u \\ (m - X_{\dot{u}})v & -(m - X_{\dot{u}})u & 0 \end{bmatrix} \qquad (4.18b) $$

$$ D(\nu) = - \begin{bmatrix} X_u & 0 & 0 \\ 0 & Y_v & 0 \\ 0 & 0 & N_r \end{bmatrix} \qquad (4.18c) $$

<footnote n="2">2. Some work like [154] use $X_u = 0$ and $Y_v \neq 0$ and obtain similar results.</footnote>
With these new hypotheses, the surface vessel system becomes:

$$ \dot{x} = u \cos \psi - v \sin \psi \qquad (4.19a) $$

$$ \dot{y} = u \sin \psi + v \cos \psi \qquad (4.19b) $$

$$ \dot{\psi} = r \qquad (4.19c) $$

$$ \dot{u} = \frac{\tau_u}{m - X_{\dot{u}}} + vr + \frac{X_u}{m - X_{\dot{u}}}u \qquad (4.19d) $$

$$ \dot{v} = -ur + \frac{Y_v}{m - Y_{\dot{v}}}v \qquad (4.19e) $$

$$ \dot{r} = \frac{\tau_r}{I_0 - N_{\dot{r}}} + \frac{N_r}{I_0 - N_{\dot{r}}}r \qquad (4.19f) $$

Notice that many coupling terms have been canceled in the new expression (4.19) like in the model of the cylindrical AUV used before in this work (see chapter 1 for details). Notably, the yaw dynamics (4.19f) are now independent of the two other DOF and some yaw speed and acceleration terms have been canceled out from the surge and sway dynamics, equation (4.19d) and equation (4.19e) respectively.

For ease of writing, the following notations are introduced:

$$ \beta = \frac{X_u}{m - X_{\dot{u}}} = \frac{Y_v}{m - Y_{\dot{v}}} \qquad (4.20a) $$

$$ \gamma = \frac{N_r}{I_0 - N_{\dot{r}}} \qquad (4.20b) $$

$$ \tilde{\tau}_u = \frac{\tau_u}{m - X_{\dot{u}}} \qquad (4.20c) $$

$$ \tilde{\tau}_r = \frac{\tau_r}{I_0 - N_{\dot{r}}} \qquad (4.20d) $$

The model thus reads:

$$ \dot{x} = u \cos \psi - v \sin \psi \qquad (4.21a) $$

$$ \dot{y} = u \sin \psi + v \cos \psi \qquad (4.21b) $$

$$ \dot{\psi} = r \qquad (4.21c) $$

$$ \dot{u} = \tilde{\tau}_u + vr + \beta u \qquad (4.21d) $$

$$ \dot{v} = -ur + \beta v \qquad (4.21e) $$

$$ \dot{r} = \tilde{\tau}_r + \gamma r \qquad (4.21f) $$
### 4.3.3.1 Flatness of the hovercraft

As before, the sensitive flat output candidate is $z = [x y]^T$ as it would enable straightforward position tracking applications. In the system (4.21) the variables which need to be expressed as functions of the flat output candidate to demonstrate flatness of the system are the yaw angle $\psi$, the surge and sway speeds $u$ and $v$ and the two system inputs $\tilde{\tau}_u$ and $\tilde{\tau}_r$.

Some of the interesting steps leading to the demonstration of flatness are presented here. First the kinematic part of the model is inverted to give expressions of the speed of the vehicle in the mobile frame:

$$u = \dot{x} \cos \psi + \dot{y} \sin \psi \qquad (4.22a)$$

$$v = -\dot{x} \sin \psi + \dot{y} \cos \psi \qquad (4.22b)$$

Then, the trigonometric combinations of the first time derivatives of equation (4.21a) and equation (4.21b) are used to find formulations of $\psi$ and $\tilde{\tau}_u$.

$$\ddot{x} \cos \psi + \ddot{y} \sin \psi = \tilde{\tau}_u + \beta u \qquad (4.23a)$$

$$-\ddot{x} \sin \psi + \ddot{y} \cos \psi = \beta v \qquad (4.23b)$$

Equation (4.23b) and (4.22b) directly give:

$$\tan \psi = \frac{\ddot{y} - \beta \dot{y}}{\ddot{x} - \beta \dot{x}} \Rightarrow \psi = \operatorname{atan}\left(\frac{\ddot{y} - \beta \dot{y}}{\ddot{x} - \beta \dot{x}}\right) = \operatorname{atan}\left(\frac{Y}{X}\right) \qquad (4.24)$$

with $X = \ddot{x} - \beta \dot{x}$ and $Y = \ddot{y} - \beta \dot{y}$.

This first result is promising when compared to the general surface vehicle and equation (4.17). It shows that, because of the mass distribution and shape of the vehicle, the yaw angle is directly related to the velocity and acceleration of the vehicle in the inertial frame. This seems sensible considering the hypotheses, the vehicle naturally lines up with the vector of acceleration.

The expression of $\psi$ (4.24) then gives:

$$\cos \psi = \frac{X}{\sqrt{X^2 + Y^2}} \qquad (4.25a)$$

$$\sin \psi = \frac{Y}{\sqrt{X^2 + Y^2}} \qquad (4.25b)$$
which in turn gives:

$$u = \frac{\dot{x}X + \dot{y}Y}{\sqrt{X^2 + Y^2}} \qquad (4.26a)$$

$$v = \frac{-\dot{x}Y + \dot{y}X}{\sqrt{X^2 + Y^2}} \qquad (4.26b)$$

Then, using equation (4.22a), equation (4.23a) and equation (4.25), the first input $\tilde{\tau}_u$ is calculated as:

$$\tilde{\tau}_u = \sqrt{X^2 + Y^2} \qquad (4.27)$$

Finally, because the yaw speed and acceleration are decoupled from the two other DOF of the system, successive differentiation of equation (4.24) gives:

$$r = \frac{\dot{Y}X - \dot{X}Y}{X^2 + Y^2} \qquad (4.28a)$$

$$\tilde{\tau}_r = \frac{\ddot{Y}X - \ddot{X}Y}{X^2 + Y^2} - \frac{2(\dot{Y}X - \dot{X}Y)(X\dot{X} + Y\dot{Y})}{(X^2 + Y^2)^2} + \gamma \frac{\dot{Y}X - \dot{X}Y}{X^2 + Y^2} \qquad (4.28b)$$

All the variables of the system have been expressed as functions of the flat output candidate $z = [x \ y]^T$ and its time derivatives up to the fourth order. The hovercraft model (4.21) is thus flat and $z$ is indeed a flat output. At this point, the trajectory of the flat output must be continuously defined up to the fourth order. As seen later on, change of input allows reducing the need to the second order.

The main differences between the hovercraft model (4.21) and the general model of the underactuated surface vessel (4.16) are the hypotheses made on the numerical values of the model parameters. This goes to show that the flatness property of a system can depend on the numerical value of some parameters. Some sets of parameters can cancel or create defects preventing flatness.

### 4.3.3.2 Flatness-based controller of the hovercraft

One special feature of the hovercraft plant (4.21) is that the second input $\tilde{\tau}_r$ only controls yaw and is decoupled from the two other DOF of the system. Equation (4.21f) can be written as:

$$\tilde{\tau}_r = \ddot{\psi} - \gamma \dot{\psi} \qquad (4.29)$$

This last expression can be thought as a change of control input: instead of considering
<figure><img src="image_38.png" /><figcaption>Figure 4.2 – Bloc diagram of the flatness-based controller</figcaption></figure>

the yaw moment as an input, the yaw angle will be used as input of the simplified model<fnref n="3" />:

$$ \dot{x} = u \cos \psi - v \sin \psi \qquad (4.30a) $$

$$ \dot{y} = u \sin \psi + v \cos \psi \qquad (4.30b) $$

$$ \dot{u} = \tilde{\tau}_u + v\dot{\psi} + \beta u \qquad (4.30c) $$

$$ \dot{v} = -u\dot{\psi} + \beta v \qquad (4.30d) $$

where the two inputs $\tilde{\tau}_u$ and $\psi$ can be expressed as:

$$ \tilde{\tau}_u = \sqrt{(\ddot{x} - \beta \dot{x})^2 + (\ddot{y} - \beta \dot{y})^2} \qquad (4.31a) $$

$$ \psi = \operatorname{atan}\left(\frac{\ddot{y} - \beta \dot{y}}{\ddot{x} - \beta \dot{x}}\right) \qquad (4.31b) $$

**Remark 4.2** System (4.30) does not have the standard form of non-linear systems: the first order derivative of one of the input components $\dot{\psi}$ appears in the last two equations. This system thus belong to the class of generalized state systems. The interested reader is referred to [38] and the references therein for more information about this class of systems. While the input derivative is naturally canceled in the following development, an alternative representation of system (4.30) is given in appendix B without any derived input [47].

In this example, because no favored output component appear in equation (4.31), the controller is designed using the Brunovský state of the system [38, 84]. Following the example of section 4.1, the following state transform is a sensitive Brunovský state

<footnote n="3">3. This method is different from the two examples [154] and [135] both using second order state expansion on $\tilde{\tau}_u$ to create two fourth-order input equations of the flat output.</footnote>
candidate:

$$ \xi_{1,1} = x \qquad (4.32a) $$

$$ \xi_{1,2} = \dot{x} \qquad (4.32b) $$

$$ \xi_{2,1} = y \qquad (4.32c) $$

$$ \xi_{2,2} = \dot{y} \qquad (4.32d) $$

Using state transform (4.32), system (4.30) becomes:

$$ \dot{\xi}_{1,1} = \xi_{1,2} \qquad (4.33a) $$

$$ \dot{\xi}_{1,2} = \tilde{\tau}_u \cos \psi + \beta \xi_{1,2} \qquad (4.33b) $$

$$ \dot{\xi}_{2,1} = \xi_{2,2} \qquad (4.33c) $$

$$ \dot{\xi}_{2,2} = \tilde{\tau}_u \sin \psi + \beta \xi_{2,2} \qquad (4.33d) $$

A new input $v = [v_1 \ v_2]^T$ is introduced such as:

$$ v_1 = \tilde{\tau}_u \cos \psi \qquad (4.34a) $$

$$ v_2 = \tilde{\tau}_u \sin \psi \qquad (4.34b) $$

which yields a single solution for the original inputs:

$$ \psi_c = \operatorname{atan}\left(\frac{v_2}{v_1}\right) \qquad (4.35a) $$

$$ \tilde{\tau}_{u,c} = v_1 \cos \psi_c + v_2 \sin \psi_c \qquad (4.35b) $$

Equation (4.33) is then rewritten with the new input:

$$ \dot{\xi}_{1,1} = \xi_{1,2} \qquad (4.36a) $$

$$ \dot{\xi}_{1,2} = v_1 + \beta \xi_{1,2} \qquad (4.36b) $$

$$ \dot{\xi}_{2,1} = \xi_{2,2} \qquad (4.36c) $$

$$ \dot{\xi}_{2,2} = v_2 + \beta \xi_{2,2} \qquad (4.36d) $$

System (4.36) is expressed in the controller form [38]. An obvious simple feedback leads to the well known Brunovský form composed of two pure integrator chains, the
details are left to the reader.

Note that, in the general case, the Brunovský state transform may depend on the input and, in the case of generalized state systems, on the input derivatives. It is a specificity of this system to be generalized in its original representation equation (4.30) and standard in its Brunovský form; All derivatives of the input disappear during the state transformation.

In an exact feedforward linearizing fashion [84], the controller is calculated on this system as:

$$v_1 = \dot{\xi}_{1,2}^* + \text{PID}(e_1) - \beta \xi_{1,2}^* \quad (4.37a)$$

$$v_2 = \dot{\xi}_{2,2}^* + \text{PID}(e_2) - \beta \xi_{2,2}^* \quad (4.37b)$$

where PID is a standard PID controller function and $e_1 = \xi_{1,1}^* - \xi_{1,1}$ and $e_2 = \xi_{2,1}^* - \xi_{2,1}$ are the tracking errors. As before the upperscript * marks desired values of the variable. The controller (4.37) gives the closed-loop system:

$$\ddot{e}_1 + k_D \dot{e}_1 + k_P e_1 + k_I \chi_1 - \beta \dot{e}_1 = 0 \quad (4.38a)$$

$$\ddot{e}_2 + k_D \dot{e}_2 + k_P e_2 + k_I \chi_2 - \beta \dot{e}_2 = 0 \quad (4.38b)$$

where $\chi_i = \int_0^t e_i(\sigma)d\sigma$ and $k_P$, $k_D$ and $k_I$ are the positive gain parameters of the PID. In the present case, the closed-loop system is linear. The feedforward terms of the controller add the two damping terms $\beta \dot{e}_1$ and $\beta \dot{e}_2$ in the error dynamics. The damping parameter $\beta < 0$ thus behaves as an additional derivative gain stabilizing the system. In this example, the same gains have been chosen on the two axes as the vehicle is perfectly symmetrical. In the following applications, notably when the hypothesis of a circular hull is not met, different sets of gains on the two axes could be used.

Here, the original inputs of system (4.30) can be calculated using equation (4.35):

$$\begin{aligned} \psi_c &= \operatorname{atan}\left(\frac{\dot{\xi}_{2,2}^* + \text{PID}(e_2) - \beta \xi_{2,2}^*}{\dot{\xi}_{1,2}^* + \text{PID}(e_1) - \beta \xi_{1,2}^*}\right) \\ &= \operatorname{atan}\left(\frac{\ddot{y}^* + \text{PID}(e_y) - \beta \dot{y}^*}{\ddot{x}^* + \text{PID}(e_x) - \beta \dot{x}^*}\right) \end{aligned} \quad (4.39a)$$

$$\begin{aligned} \tilde{\tau}_{u,c} &= (\dot{\xi}_{1,2}^* + \text{PID}(e_1) - \beta \xi_{1,2}^*) \cos \psi_c + (\dot{\xi}_{2,2}^* + \text{PID}(e_2) - \beta \xi_{2,2}^*) \sin \psi_c \\ &= (\ddot{x}^* + \text{PID}(e_x) - \beta \dot{x}^*) \cos \psi_c + (\ddot{y}^* + \text{PID}(e_y) - \beta \dot{y}^*) \sin \psi_c \end{aligned} \quad (4.39b)$$

The final controller (4.39) indeed shows the same symmetry characteristics as the rest
of the hovercraft system. Because no input has been favored in the inputs calculations, the controllability of the system is ensured.

**Remark 4.3** Note that the final controller (4.39) has the same shape as the flatness input equations (4.31). While it would have not been rigorous to directly use the equations of flatness to build the controller on this system for the various reasons exposed before, the alternative method without using the Brunovský state would have given a similar controller structure.

**Remark 4.4** In the presence of yaw disturbance or when considering actuators limitations, it is necessary to add an extra level of control on yaw. The yaw subsystem (Eqs.(4.21c),(4.21f)) being flat with $\psi$ as the flat output, the yaw controller can be:

$$ \tilde{\tau}_{r,c} = \dot{r}^* + k_d^\psi \dot{e}_\psi + k_p^\psi e_\psi + k_i^\psi \chi_\psi - \gamma r^* \quad (4.40) $$

with $e_\psi = \psi_c - \psi$, $r^* = \dot{\psi}_c$, $\dot{r}^* = \ddot{\psi}_c$ and $k_p^\psi, k_i^\psi$ and $k_d^\psi$ are appropriate gain parameters. The yaw angle $\psi$ can be measured or calculated using the equation of flatness (4.24). Any other autopilot could be used here provided that they are faster than the main flatness-based control loop to ensure the behavior of the cascaded system.

Before adding the iPID in the flatness-based controller, it is tested on a SV model with a conventional PID. The work of [85] shows that flatness-based control is naturally robust to a certain amount of model approximation (10% in the reference) and that the approximation of some of the terms of the model can act favorably for the stability of the closed-loop system. It is then reasonable to expect the controller (4.39) to give good results when applied to the SV. Of course in the SV model, added-mass and damping are different on the surge and sway axis: $X_u \neq Y_v$ and $X_u \neq Y_v$. The parameter $\beta$ of equation (4.39) would then be chosen as the smaller of the two damping parameters so that the controller does not rely on a false natural damping effect. The simulation results presented in section 4.5.2 show that the flatness based controller (4.39) associated with the inner loop (4.40) gives very good trajectory tracking results on both the hovercraft and the surface vessel. The main limits of this controller as demonstrated in section 4.5.2.4 is the behavior outside of the neighborhood of the trajectory. This weakness is compensated in the following section with an additional layer of Intelligent PID control.
## 4.4 Association with Intelligent PID

In this Section, the results obtained on the hovercraft system are extended to all surface vessels. To do so, the flatness-based controller developed in the previous section is associated with Model Free Control and Intelligent PIDs (iPIDs). The idea is to ally the natural robustness of flatness-based control with the adaptability of the iPID to design a controller which can be applied to any surface vessel. Therefore, the controller is calculated as if the system to control were a hovercraft (with the hypotheses of circular shape and symmetrical mass distribution) but is applied to any surface vessel model.

As exposed in section 2.5.1, iPIDs are a wide-spread model-free control solution successfully applied to a multitude of various systems. Details about the theory behind iPIDs can be found in [49]. The idea is to approximate the system with an ultra-local model containing an adaptive term estimating all the non-modeled effects acting on the system as a single number (or vector). Here, the iPID is associated with the nominal control calculated with flatness, is applied to the Brunovský state representation of the hovercraft system and then propagated in the model of the vehicle.

Intelligent PIDs have yet to be implemented on the underactuated SV. The SV being ill-actuated with respect to the task (lack of one translation), iPID alone will not be sufficient to complete the task. A compensation mechanism is needed to control the lacking translation using the rotation. It is therefore associated with flatness to create this compensation mechanism.

### 4.4.1 Implementation of the iPID

Recall the Brunovský representation of the hovercraft:

$$ \dot{\xi}_{1,1} = \xi_{1,2} \qquad (4.41a) $$

$$ \dot{\xi}_{1,2} = v_1 + \beta\xi_{1,2} \qquad (4.41b) $$

$$ \dot{\xi}_{2,1} = \xi_{2,2} \qquad (4.41c) $$

$$ \dot{\xi}_{2,2} = v_2 + \beta\xi_{2,2} \qquad (4.41d) $$

with $\xi_{1,1} = x$, $\xi_{1,2} = \dot{x}$, $\xi_{2,1} = y$ and $\xi_{2,2} = \dot{y}$.

In order to associate the flatness-based controller calculated in the previous section
with the iPID, the control inputs are broken down into two parts:

$$v_1 = v_1^* + \delta v_1 \qquad (4.42a)$$

$$v_2 = v_2^* + \delta v_2 \qquad (4.42b)$$

where $v_1^*$ and $v_2^*$ are the nominal controller calculated with flatness and $\delta v_1$ and $\delta v_2$ are the iPID contribution. The idea here is to use the iPID to control the vehicle around its nominal trajectories dictated by the flatness-based controller. This is somewhat similar to the idea behind SMC where the system is oscillating around the sliding surface representing the desired equilibrium.

Considering the system (4.41), the nominal flatness-based controllers are given as:

$$v_1^* = \dot{\xi}_{1,2}^* - \beta \xi_{1,2}^* = \ddot{x}^* - \beta \dot{x}^* \qquad (4.43a)$$

$$v_2^* = \dot{\xi}_{2,2}^* - \beta \xi_{2,2}^* = \ddot{y}^* - \beta \dot{y}^* \qquad (4.43b)$$

Then, applying the controller (4.42) into the system (4.41), a new system based on the tracking error and controlled with the iPID controllers $\delta v_1$, $\delta v_2$ is created:

$$\ddot{x} = v_1^* + \delta v_1 + \beta \dot{x} = \ddot{x}^* - \beta \dot{x}^* + \delta v_1 + \beta \dot{x} \Rightarrow \ddot{e}_x = \beta \dot{e}_x - \delta v_1 \qquad (4.44a)$$

$$\ddot{y} = v_2^* + \delta v_2 + \beta \dot{y} = \ddot{y}^* - \beta \dot{y}^* + \delta v_2 + \beta \dot{y} \Rightarrow \ddot{e}_y = \beta \dot{e}_y - \delta v_2 \qquad (4.44b)$$

where the subscript * denotes desired values of the variable and $e_x = x^* - x$ and $e_y = y^* - y$.

Two iPID controllers are therefore designed to control the error system in the vicinity of the trajectory. The system is composed of two second order decoupled subsystems thus second order ultra-local models are used. The two error signals are considered as measurements in the two ultra-local models:

$$\ddot{e}_x = \alpha_x \delta v_1 + F_x \qquad (4.45a)$$

$$\ddot{e}_y = \alpha_y \delta v_2 + F_y \qquad (4.45b)$$

where $F_x$ and $F_y$ are the two adaptive functions estimating the unmodeled terms and $\alpha_x$ and $\alpha_y$ are two control parameters.

Following the reasoning in section 2.5, the estimation of $F_x$ and $F_y$ is done using ALIEN filters [58, 57]. For the second order ultra-local models, $F_x$ and $F_y$ are estimated
as:

$$ \hat{F}_x = \frac{5!}{2T^5} \int_0^T \left[ \left[ (T-\tau)^2 - 4(T-\tau)\tau + \tau^2 \right] x(\tau) - \alpha_x \frac{(T-\tau)^2}{2} \tau^2 \delta v_1(\tau) \right] d\tau \quad (4.46a) $$

$$ \hat{F}_y = \frac{5!}{2T^5} \int_0^T \left[ \left[ (T-\tau)^2 - 4(T-\tau)\tau + \tau^2 \right] y(\tau) - \alpha_y \frac{(T-\tau)^2}{2} \tau^2 \delta v_2(\tau) \right] d\tau \quad (4.46b) $$

The adaptive terms $\hat{F}_x$ and $\hat{F}_y$ are considered constant on one time sample and are calculated once for each sample. The time dependency in the expressions of $\hat{F}_x$ and $\hat{F}_y$ is omitted for clarity.

Because the two iPIDs are calculated on the error system, the desired values in both $e_x$ and $e_y$ are all zero: $e_x^* = \dot{e}_x^* = \ddot{e}_x^* = 0$ and $e_y^* = \dot{e}_y^* = \ddot{e}_y^* = 0$. The two control inputs of the ultra-local models are then:

$$ \begin{aligned} \delta v_1 &= \frac{1}{\alpha_x} \left( \ddot{e}_x^* - \hat{F}_x + K_d^x (\dot{e}_x^* - \dot{e}_x) + K_p^x (e_x^* - e_x) \right) \\ &= \frac{1}{\alpha_x} \left( -\hat{F}_x + K_d^x (-\dot{e}_x) + K_p^x (-e_x) \right) \end{aligned} \quad (4.47a) $$

$$ \begin{aligned} \delta v_2 &= \frac{1}{\alpha_y} \left( \ddot{e}_y^* - \hat{F}_y + K_d^y (\dot{e}_y^* - \dot{e}_y) + K_p^y (e_y^* - e_y) \right) \\ &= \frac{1}{\alpha_y} \left( -\hat{F}_y + K_d^y (-\dot{e}_y) + K_p^y (-e_y) \right) \end{aligned} \quad (4.47b) $$

with $K_D^x$, $K_P^x$, $K_D^y$ and $K_P^y$ strictly positive gain parameters. The complete controller is thus:

$$ v_1 = \ddot{x}^* - \beta \dot{x}^* + \frac{1}{\alpha_x} \left( -\hat{F}_x + K_d^x (-\dot{e}_x) + K_p^x (-e_x) \right) \quad (4.48a) $$

$$ v_2 = \ddot{y}^* - \beta \dot{y}^* + \frac{1}{\alpha_y} \left( -\hat{F}_y + K_d^y (-\dot{e}_y) + K_p^y (-e_y) \right) \quad (4.48b) $$

Figure 4.3 shows the final flatness and iPD controller applied to the Brunovský state representation of the system.

Note that different sets of gain parameters are used on the two axes. Without the even mass distribution approximation of the hovercraft system, the gain parameters have no reason to be the same on the two axes. Practically, they could be chosen equal in some cases.

Also, no integral term is necessary in equation (4.48). The work of [127] and [48] shows that the adaptive terms $F_x$ and $F_y$ cover the work of the integral term of a conventional
<figure><img src="image_39.png" /><figcaption>Figure 4.3 – Bloc diagram of the flatness and iPD controller applied to the Brunovský state representation.</figcaption></figure>

PID. In facts, intelligent proportional derivative controllers as this one are shown to be equivalent to conventional PIDs.

## 4.5 Simulations of the flatness-based controllers

This section presents simulations of the two flatness-based controllers developed on the fully-actuated and underactuated vehicles. In the fully-actuated case, the flatness-based controller is compared to a feedback linearizing PID-based controller and a sliding mode controller. It shows very good trajectory tracking performance and robustness external disturbance.

In the underactuated case, the conventional flatness-based controller and the association with iPID are shown to be very effective on both the hovercraft and the generic surface vessel. Both controller show good robustness to external disturbances, measurement noise and model approximations. The iPID is shown to compensate the behavior of conventional flatness when the systems gets too far from the desired path.

### 4.5.1 Fully-actuated AUV

In the fully-actuated case, the flatness-based controller is compared to a feedback linearizing PID-based controller and a sliding mode controller. These controllers are compared on seabed scanning mission consisting in horizontal rails at constant depth. The task requires the six DOF, the center of the vehicle must actively track the position of the target moving on the trails as well as following the attitude trajectory. To confirm the theoretical performances of the flatness-based controller it is first tested on a reference scenario with no external disturbance or approximation and then evaluated with external
disturbances, estimation errors of the model parameters and initial errors.

The control laws used for comparison are recalled here. The first controller is a standard feedback linearizing, PID-based controller calculated in the inertial frame.

$$ \tau_{PID} = M (\mathbf{J}(\eta)^{-1}(\ddot{\eta}^* + \mathbf{PID}(e_{\eta})) + \dot{\mathbf{J}}(\eta, \dot{\eta})^{-1}\dot{\eta}) + \mathbf{C}(\nu)\nu + \mathbf{D}(\nu)\nu + g(\eta) \quad (4.49) $$

In equation (4.49), $PID(e_{\eta})$ denotes six decoupled PID controllers, one per DOF, calculated in the inertial frame on the position and orientation errors contained in $e_{\eta}$. In this example, the same gain parameters are used on all six DOF. Allegedly, using a different set of gain parameters on each DOF would allow finer tuning of the performances on each axis. Yet, the same of gains used in this example shows very good performances on all six DOF.

The second control law is the Sliding Mode controller presented in section 2.4 [40] and adapted to this vehicle. It is composed of six decoupled sliding mode controllers, one per DOF. To avoid chattering and as is usually done, the signum function was replaced by a saturation function. Note that, to match with the greater number of references, the control surfaces are calculated with respect to the position and orientation error vector. No integral term is added and therefore steady state error can appear in the presence of unknown external disturbances (See appendix A).

$$ \tau_{SMC} = \tilde{M}(\eta)(\ddot{\eta}^* + \lambda\dot{e}_{\eta} + K\text{sat}(\sigma)) + \tilde{C}(\eta, \nu)\dot{\eta} + \tilde{D}(\eta, \nu)\dot{\eta} + g(\eta) \quad (4.50) $$

In this example, the gain matrices $\lambda$ and $K$ are diagonal and the same gain parameters are used for all six DOF. The expressions of the model matrices expressed in the inertial frame $\tilde{M}$, $\tilde{C}$ and $\tilde{D}$ are given in Equation (4.10).

For the sake of comparison, the flatness-based controller is recalled here:

$$ \tau_{Flat} = \tilde{M}(\eta^*)(\ddot{\eta}^* + \mathbf{PID}(e_{\eta})) + \tilde{C}(\eta^*, \nu^*)\dot{\eta}^* + \tilde{D}(\eta^*, \nu^*)\dot{\eta}^* + g(\eta^*) \quad (4.51) $$

The first difference appearing between the three controllers is the linearizing terms. As described before, one of the main assets of flatness-based control is that the conventional linearizing terms of a feedback linearizing controller are substituted with the nominal value of the controller, creating increased robustness. One the other hand, the linearizing terms of both the PID-based controller and the SM controller are calculated at the current
state of the vehicle. Besides, all three controllers show very similar structures. The three control laws are calculated using error vectors expressed in the inertial frame.

All the following tests are performed on the RSM vehicle equipped with six independent fixed thrusters and presented in figure 1.3. The control parameters of the three controllers are given in table 4.1. All the gain matrices are diagonal with equal factors on all the rows.

<table><caption>Table 4.1 – Simulation parameters - fully actuated vehicle</caption><thead><tr><th>Item</th><th>Value</th><th>Item</th><th>Value</th></tr></thead><tbody><tr><td>Trajectory</td><td>Seabed Scanning</td><td>Nominal Speed</td><td>1.5m.s<sup>-1</sup></td></tr><tr><td>Simulation time sample</td><td>0.05ms</td><td>Vehicle Shape</td><td>Cylinder</td></tr><tr><td colspan="4">Flatness-based controller</td></tr><tr><td>k<sub>p</sub></td><td>20</td><td>k<sub>i</sub></td><td>5</td></tr><tr><td>k<sub>d</sub></td><td>8</td><td></td><td></td></tr><tr><td colspan="4">PID</td></tr><tr><td>k<sub>p</sub></td><td>20</td><td>k<sub>i</sub></td><td>5</td></tr><tr><td>k<sub>d</sub></td><td>8</td><td></td><td></td></tr><tr><td colspan="4">Sliding Mode Controller</td></tr><tr><td>K</td><td>5</td><td>λ</td><td>10</td></tr></tbody></table>

The gain parameters of the PID-based controller and those of the Flatness-based controllers have been purposely chosen equal to allow better comparison of the linearizing techniques. The nominal speed of the following tests has been chosen to ensure that the thrusters of these vehicles stay far from their maximum thrusts. At nominal speed, the thrusters generate about 35% of their maximal thrust.

It is worth mentioning that all three controllers have equivalent energy consumption on each of the following tasks.

### 4.5.1.1 Base case scenario

The base case scenario of this study is a trajectory tracking task with no external disturbance or model approximation. The vehicle is placed on the task at the initial
<figure><img src="image_40.png" /><figcaption>Figure 4.4 – Comparison of the position errors for the three controllers. Red: PID - Green: SMC - Blue: Flatness-based</figcaption></figure>

time so there is no initial error. In facts, the trajectory is composed of six independent trajectories, one per DOF, continuously defined up to the fourth order. They are described with connected polynomials to ensure continuity up to the fourth order.

Figures 4.4 and 4.5 respectively show the position and orientation errors of the three controllers. As expected, the three controllers perform equally well on the six DOF during the complete experiment. Very slightly larger errors appear on the **x**<sub>o</sub> and **y**<sub>o</sub> axis of the PID-based controller (Red line on figure 4.4.(a) and figure 4.4.(b)) and on the yaw error (Red line on figure 4.5.(c)). Considering that the gain parameters are the same for the FBLN and Flatness-based controllers, these difference (magnitudes of a centimeter and a degree respectively) may be attributed to the linearizing terms.

4.5.1.2 Initial error

As exposed in the previous sections, Flatness-based control has notably been designed
to be used at the vicinity of the desired trajectory. Using trajectory values in the linearizing
terms of the controller implies that the nonlinearities of the closed-loop system cancel only
when the system is on trajectory. Otherwise, some non linear terms remain in the closed-
<figure><img src="image_41.png" /><figcaption>Figure 4.5 – Comparison of the orientation errors for the three controllers. Red: PID - Green: SMC - Blue: Flatness-based</figcaption></figure>

loop system.

To illustrate the behavior of the Flatness-based controller when the system is not on the trajectory, two tests are performed with initial position error on the **y**<sub>o</sub> axis. For the first test, depicted on figure 4.6, the initial error is 1 m while for the second test depicted on figure 4.7, the initial error is 5 m. In the initial configuration, the error is on the **y**<sub>B</sub> axis of the mobile frame, all other DOF (up to the fourth order) are initiated at the desired value. The vehicle is then supposed to perform a sway motion whilst maintaining proper depth, surge speed and orientation.

In the first case, with the 1m initial error, all three vehicle converge on the trajectory. It
is worth noting that the Flatness-based and PID-based controllers behave almost equally
and converge faster than the SM controller but at the cost of a greater disturbance on
the **x**<sub>o</sub> axis (and greater orientation disturbances, not given here).

On the other hand, with the 5m initial error, the flatness-based controller does not
converge on the trajectory. In facts, the **y**<sub>o</sub> is not canceled but oscillates around the initial
value, slightly diverging. More concerning, the controller generates great disturbances on
other DOF as shown on figure 4.7.(a) and figure 4.7.(c): both errors on the **x**<sub>o</sub> and **z**<sub>o</sub> axes
diverge. The PID-based and SM controllers eventually converge to the trajectory with the
<figure><img src="image_42.png" /><figcaption>Figure 4.6 – Comparison of the position errors for the three controllers with a 1m initial error on the $\mathbf{y}_o$ axis. Red: PID - Green: SMC - Blue: Flatness-based</figcaption></figure>

<figure><img src="image_43.png" /><figcaption>Figure 4.7 – Comparison of the orientation errors for the three controllers with a 5m initial error on the $\mathbf{y}_o$ axis. Red: PID - Green: SMC - Blue: Flatness-based</figcaption></figure>
<figure><img src="image_44.png" /><figcaption>Figure 4.8 – Comparison of the position errors for the three controllers with a 0.75m/s current on $x_0$ axis. Red: PID - Green: SMC - Blue: Flatness-based</figcaption></figure>

5m initial error.

This example shows that the flatness-based controller performs as well as conventional controllers in the neighborhood of the trajectory but does not converge when the vehicle is too far from the trajectory. Of course, the numerical size of the boundary layer around the trajectory allowing convergence of the system depends on the system itself. Therefore, if the vehicle were to get outside of this convergence boundary around the trajectory (here, outside of a 5m range from the target), the flatness-based controller would not allow correcting the error and a new trajectory should be calculated to catch up.

### 4.5.1.3 Robustness to external disturbances

To evaluate the robustness to external disturbances, an ocean current is added in simulation. This current is not known by the controller and therefore not compensated. In this example, a constant irrotational current is used. It is aligned with the $x_0$ axis and its velocity is half of the nominal velocity required on the trajectory: 0.75 m/s.

As shown on figure 4.8 the PID-based controller and the flatness-based controller perform as well in this case. The errors shown on figure 4.8 for these two controllers are
<figure><img src="image_45.png" /><figcaption>Figure 4.9 – Comparison of the orientation errors for the three controllers with a 0.75m/s current on $x_0$ axis. Red: PID - Green: SMC - Blue: Flatness-based</figcaption></figure>

the same as in the base case figure 4.4. Note that the SM controller shows the expected steady state error on the $x_0$ axis (Green line on figure 4.8.(a)).

Figure 4.9 shows that the flatness-based controller does not deviate much in roll (figure 4.9.(a)) but a 15 degrees deviation appears on the yaw angle on figure 4.9.(c). The yaw deviation is slightly larger than the one appearing with the PID-based controller. The PID-based controller also demonstrates some pitch oscillations when the vehicle is facing the current that do not appear when using the Flatness-based controller. Here again, the SM controller shows non zero steady state errors in both roll and yaw.

Overall, the Flatness-based controller shows good robustness to external disturbances in the fully-actuated case. In fact, as long as the vehicle stays in the vicinity of the trajectory, the integral term of the controller ensures convergence of the (almost linear) closed-loop system when facing unknown constant disturbances. The difference of pitch oscillations between the PID-based controller and the Flatness-based controller could be interpreted as a proof of the effect of the stabilizing non-linear terms resulting from the feedforward linearizing method. Further tests are necessary to evaluate the role of each of the terms of the closed-loop system created by the flatness-based controller but the simulations show that the flatness-based controller is as good as the other controllers
from the literature.

## 4.5.2 Underactuated surface vessel

This section presents the trajectory tracking simulation results obtained with the two flatness-based controllers introduced in section 4.3.3 and section 4.4. Both controllers are calculated using the hovercraft model considering a circular hull shape and an even mass distribution. They are applied first to a hovercraft and then to the model of a generic surface vessel. The surface vessel does not meet the hypothesis of circular shape and mass distribution of the hovercraft. The conventional hypothesis for estimation of the added mass and damping parameters are used (see chapter 1 for details). One notable difference is that the surge and sway added masses and damping parameters are not equal on the surface vessel and additional coupling terms appear between the sway and yaw in both the mass and damping matrices. The obvious flatness defects due to the shape of the surface vessel have been described in section 4.3.

The control laws are recalled here. Both controllers are calculated on the Brunovský representation of the system and then transformed into the inputs of the original system. The conventional flatness-based controller is:

$$v_1 = \ddot{x}^* + \text{PID}(e_x) - \beta \dot{x}^* \quad (4.52a)$$

$$v_2 = \ddot{y}^* + \text{PID}(e_y) - \beta \dot{y}^* \quad (4.52b)$$

where PID is a regular PID function with strictly positive gain parameters.

The second controller is the association of a flatness-based controller and an iPID. The inputs are calculated as the sum of the nominal control based on flatness and the iPID applied to the error system. The controller reads:

$$v_1 = \ddot{x}^* - \beta \dot{x}^* + \frac{1}{\alpha_x} \left( -\hat{F}_x + K_d^x(-\dot{e}_x) + K_p^x(-e_x) \right) \quad (4.53a)$$

$$v_2 = \ddot{y}^* - \beta \dot{y}^* + \frac{1}{\alpha_y} \left( -\hat{F}_y + K_d^y(-\dot{e}_y) + K_p^y(-e_y) \right) \quad (4.53b)$$

$$\hat{F}_x = \frac{5!}{2T^5} \int_0^T \left[ \left[ (T - \tau)^2 - 4(T - \tau)\tau + \tau^2 \right] x(\tau) - \alpha_x \frac{(T - \tau)^2}{2} \tau^2 \delta v_1(\tau) \right] d\tau \quad (4.53c)$$

$$\hat{F}_y = \frac{5!}{2T^5} \int_0^T \left[ \left[ (T - \tau)^2 - 4(T - \tau)\tau + \tau^2 \right] y(\tau) - \alpha_y \frac{(T - \tau)^2}{2} \tau^2 \delta v_2(\tau) \right] d\tau \quad (4.53d)$$
Little literature exists on the tuning of the control parameters $\alpha_x$ and $\alpha_y$. The rule of thumb is to choose $\alpha_x$ ad $\alpha_y$ so that the inputs $\delta v_1$ and $\delta v_2$ are approximately the same order of magnitude as second order derivative of the measurements $\ddot{e}_x$ and $\ddot{e}_y$ respectively. In this case, because the two iPIDs are calculated on error systems, the expected values for $\ddot{e}_x$ and $\ddot{e}_y$ is zero. The control parameters $\alpha_x$ and $\alpha_y$ have then been chosen equal to 1. The damping parameters are chosen as $\beta = -12$ and $\gamma = -4$.

The controls of the original system are then obtained using:

$$ \psi_c = \operatorname{atan}\left(\frac{v_2}{v_1}\right) \qquad (4.54a) $$

$$ \tilde{\tau}_{u,c} = v_1 \cos \psi_c + v_2 \sin \psi_c \qquad (4.54b) $$

$$ \tilde{\tau}_{r,c} = \dot{r}^* + k_d^\psi \dot{e}_\psi + k_p^\psi e_\psi + k_i^\psi \chi_\psi - \gamma r^* \qquad (4.54c) $$

Note that the following simulation results have been obtained on a much simpler simulator than the other simulations presented in this work. They are preliminary results of the methods but some more refined work is still needed. The simplified simulator does not take limitations of variation of inputs into account but the control efforts are upper bounded with the arbitrary chosen maximal values of $\tilde{\tau}_{u,c}^{max} = 100$ N and $\tilde{\tau}_{r,c}^{max} = 100$ Nm.

### 4.5.2.1 Ideal case

The two controllers are first tested in the ideal case. They are applied to the hovercraft without noise and without external disturbance. The task is defined as a succession of three segments in the horizontal plane. The first one is aligned with the $x_0$ axis, the second one with the $y_0$ axis and the third one is diagonal. On each segment, the vehicle must accelerate to reach the nominal speed of $1 \text{ m s}^{-1}$ then decelerate so that it arrives at the next turning point with zero velocity. This task is referred to as the Lawn Mower task referring to the behavior of an autonomous lawn mower robot.

The PID of the flatness-based controller, iPD and PID of the low-level $\psi$ controller are tuned using a simplified pole placing method. The poles of each system are chosen to be in the same place so that the three controller can be tuned choosing only one characteristic time parameter. The sets of gains are chosen to ensure that the inner loop controlling the $\psi$ angle is always faster than the outer loop.

Figure 4.10 shows the behavior of the two vehicles on the task. The trajectory may appear discontinuous on figure 4.10 but it is in fact continuous up to the fourth order. Both controllers allow almost perfect tracking of the trajectory in the ideal case as expected.
<figure><img src="image_46.png" /><figcaption>Figure 4.10 – Comparison of the trajectory of the flatness-based controllers in the ideal case - Red: conventional flatness, Blue: iPID, Black: Reference</figcaption></figure>