<figure><img src="image_1.png" /></figure>

HAL
open science

# Energy-Efficient Configuration and Control Allocation for a Dynamically Reconfigurable Underwater Robot

Huu Tho Dang, Lionel Lapierre, René Zapata, Benoît Ropars

<figure><img src="image_2.png" /></figure>

## To cite this version:

Huu Tho Dang, Lionel Lapierre, René Zapata, Benoît Ropars. Energy-Efficient Configuration and Control Allocation for a Dynamically Reconfigurable Underwater Robot. Sensors, 2023, 23 (12), pp.5439. 10.3390/s23125439. hal-05288218

HAL Id: hal-05288218

https://hal.science/hal-05288218v1

Submitted on 29 Sep 2025

HAL is a multi-disciplinary open access
archive for the deposit and dissemination of sci-
entific research documents, whether they are pub-
lished or not. The documents may come from
teaching and research institutions in France or
abroad, or from public or private research centers.

L'archive ouverte pluridisciplinaire HAL, est
destinée au dépôt et à la diffusion de documents
scientifiques de niveau recherche, publiés ou non,
émanant des établissements d'enseignement et de
recherche français ou étrangers, des laboratoires
publics ou privés.

<figure><img src="image_3.png" /></figure>

Distributed under a Creative Commons Attribution 4.0 International License
Article

# Energy-Efficient Configuration and Control Allocation for a Dynamically Reconfigurable Underwater Robot

Tho Dang <fnref n="1" />*, Lionel Lapierre <fnref n="1" />*, Rene Zapata <fnref n="1" /> and Benoit Ropars <fnref n="2" />

**Abstract:** A dynamically reconfigurable underwater robot, which can vary its configuration during a mission, would be useful for confined environment exploration and docking because of its versatility. A mission can be performed by choosing among different configurations, and the energy cost may increase, owing to the reconfigurability of the robot. Energy saving is the critical issue in long-range missions with underwater robots. Moreover, control allocation must be considered for a redundant system and input constraints. We propose an approach for an energy-efficient configuration and control allocation for a dynamically reconfigurable underwater robot that is built for karst exploration. The proposed method is based on sequential quadratic programming, which minimizes an energy-like criterion with respect to robotic constraints, i.e., mechanical limitations, actuator saturations, and a dead zone. The optimization problem is solved in each sampling instant. Two popular tasks for underwater robots, i.e., path-following and station-keeping (observation) problems, are simulated, and the simulation results show the efficiency of the method. Moreover, an experiment is carried out to highlight the results.

**Keywords:** autonomous underwater robot; dynamically reconfigurable underwater robot; control allocation; optimization

<figure><img src="image_4.png" /></figure>

check for
updates

**Citation:** Dang, T.; Lapierre, L.; Zapata, R.; Ropars, B. Energy-Efficient Configuration and Control Allocation for a Dynamically Reconfigurable Underwater Robot. *Sensors* **2023**, *23*, 5439. https://doi.org/10.3390/s23125439

Academic Editors: Thor I. Fossen and Enrico Meli

Received: 28 March 2023

Revised: 21 May 2023

Accepted: 6 June 2023

Published: 8 June 2023

<figure><img src="image_5.png" /></figure>

**Copyright:** © 2023 by the authors. Licensee MDPI, Basel, Switzerland. This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution (CC BY) license (https://creativecommons.org/licenses/by/4.0/).

# 1. Introduction

## 1.1. A Dynamically Reconfigurable Underwater Robot and Perspectives

In robotic fields, reconfigurable robots are an attractive area because of their versatility. They can change their shape or configuration corresponding to specific mission requirements. Therefore, the building cost can be reduced by one robot performing several tasks. Moreover, reconfigurable robots can be applied for complex tasks requiring adaptive configurations, such as karst exploration or space applications. Robustness is also an advantage of reconfigurable robots in terms of the flexibility. Overviews of these aspects and other issues of modular self-reconfigurable robot systems are available in the literature [1,2].

A dynamically reconfigurable underwater robot was built in our laboratory at Montpellier University. Readers can refer to [3] for more details. The robot consists of seven thrusters (three forward and four backward thrusters), and its configuration can be dynamically varied. Figure 1 shows some configurations of the robot, i.e., the robot has an open state for the forward branch and a close state for the backward branch, called an open-close state. The similarities are shown in Figure 1b,c, corresponding to close-close and open-open states, respectively. The change in the robot configuration can be viewed at https://youtu.be/yBBCu1z3q-0 (accessed on 7 February 2023). In the close-close state, it operates as a torpedo-shaped robot; in the open-open state, it operates as an isotropic system. Our robot can operate as an over-actuated system.

<footnote n="1">¹ Laboratory of Informatics, Robotics and MicroElectronics (LIRMM) (UMR 5506 CNRS—UM), Université Montpellier, 161 rue Ada, CEDEX 5, 34392 Montpellier, France; zapata@lirmm.fr</footnote>

<footnote n="2">² Reeds Company, 199 rue Hélène Boucher, 34170 Castelnau-Le-Lez, France; ropars@lirmm.fr</footnote>

<footnote>* Correspondence: danghuu@lirmm.fr (T.D.); lapierre@lirmm.fr (L.L.)</footnote>
<figure><img src="image_6.png" /><figcaption>**Figure 1.** A 3D model of dynamically reconfigurable robot with three different configurations [3]. (a) Robot in open-close state. (b) Robot in close-close state. (c) The robot in open-open state.</figcaption></figure>

One of the challenges of an over-actuated system is control allocation, in which the distribution of the applied forces on actuators, the so-called applied force vector, is found when a desired control vector (output from a controller) is given. In the following, we describe control allocation methods for an over-actuated system.

## 1.2. *Control Allocation*

The basic property of an over-actuated system is that the number of actuators is larger than the controllable degrees of freedom (DOFs). The problem is how to map the desired actuation on the DOFs to forces on the actuators through a *configuration matrix*. In the literature, two approaches are developed to solve this problem. The first method is to divide the control design into two levels. In the first level, the control laws for each DOF are designed. The outputs of this level, called the desired control vector, are the inputs of the second level. In the second level, a control allocation algorithm is designed to assign the control inputs for actuators to optimize one or some cost functions with respect to redundancy and actuator limitations. The problem at the second level is called the control allocation (CA) problem. With the second method, the control inputs (normally with constraints) are directly considered in the control design process. This issue arises in the model predictive control (MPC) method because control allocation is considered a constraint in the MPC formulation. However, this increases the computational cost, so it is the most challenging issue in the MPC problem.

The control allocation problem is one of the main tasks in the control design of over-actuated or redundant systems. Normally, the actuators of a system are constrained with mechanical and electrical limitations, such as saturation or a dead zone. The role of the control allocation block in a control loop is displayed in Figure 2, in which the input is the desired control vector ($F_B^d$), and the output is the applied force vector ($F_m$).
<figure><img src="image_7.png" /><figcaption>**Figure 2.** Control allocation block in general control loop.</figcaption></figure>

The many available control allocation methods are divided into two groups: pseudo-inverse- and optimization-based methods, and with or without constraints. Without constraints, the problem is easier. However, the unconstrained control allocation problem provides the basic ideas for many constrained control allocation problems. Most of the control allocation methods are based on optimization techniques, either explicitly or implicitly. Depending on the application, the appropriate control allocation method is chosen.

Some surveys have been conducted on the control allocation problem in recent years. In [4], the authors compared many control allocation algorithms with closed- and open-loop measures. In [5], the authors evaluated the performance and computational cost of the optimization methods of the control allocation problem. In [6,7], control allocation methods for ships and underwater vehicles were investigated. A survey was published in 2013 [8], in which many control allocation methods and applications were presented and discussed. With the advances in neural networks (NNs), NN-based control allocation approaches have been developed [9,10].

## 1.3. *The Singularity of Control Allocation*

As mentioned above, many approaches can be used to solve the CA problem. However, in most cases, a configuration matrix is constant and remains unchanged during the robot operation. When the configuration matrix is varied, it may yield a singular or a near-singular configuration; therefore, some DOFs are not controllable. This was previously discussed [11] by researchers who proposed an approach to penalize the singularity of the configuration. However, some advantages of the singular configuration have been addressed, such as when facing disturbances and to achieve optimal energy. Owing to [12], we can investigate different control allocation methods for near-singular configurations. In this situation, the minimum singular value of the configuration matrix is too small. This yields a pseudo-inverse that is too large (which is easily observed with the singular value decomposition of the configuration matrix) and causes a large error if pseudo-inverse-based CA methods are used. Hence, optimization-based CA methods are suitable for dynamically reconfigurable robots.

## 1.4. *Control Allocation with Varying Configuration Matrix*

Control allocation methods with a varying configuration matrix have been introduced [13–15] for fault tolerance, in which the control performance is guaranteed when the efficiency of the actuators is lost. In another direction, the configuration matrix contains variables that must be found to minimize an objective function, which is normally power consumption. In the literature, this concept was only implicitly introduced in one study [11], in which azimuths were found to optimize energy consumption. In this study, the problem was formulated and approximated as locally convex quadratic programming. In particular, a slack variable was added to guarantee that the optimization problem always had a feasible solution and, in each sample, the nontrivial part (updating part) of the optimization problem was found by linearizing the objective function and constraints on the optimal solution of the previous sample.
For dynamically reconfigurable robots, the configuration matrix can be varied during robot operations. One question is how to achieve an optimal energy criterion with respect to the parameters of the configuration matrix and the control allocation problem. Motivated by such robots, a real-time technique in nonlinear MPC, so-called one-iteration optimization [16], and optimization-based control allocation methods, we developed an approach to achieve energy-efficient configuration and control allocation, which is different from the aforementioned method, for a dynamically reconfigurable robot that can vary its configuration during missions. The main contributions of the paper are summarized as follows:

1. Propose an energy-efficient configuration problem for a dynamically reconfigurable robot with respect to its constraints.

2. Propose an integration of a one-iteration optimization technique and a control allocation method to solve the energy-efficient configuration problem.

The remainder of this paper is organized as follows: basic notations are summarized in Appendix B. The energy-efficient configuration and control allocation problem are presented in Section 2. A proposed solution is introduced in Section 3. The simulation results are shown and discussed in Section 4. The experimental result is discussed in Section 5; finally, concluding remarks are provided in Section 6.

## 2. Energy-Efficient Configuration and Control Allocation Problem

Without loss of generality and to ensure ease of understanding, our robot is used to formulate the problem. Some additional notations are illustrated in Figure 3, i.e., body frame, $X_B, Y_B, Z_B$; linear velocities and angular rates expressed in body frames $u, v, w$ and $p, q, r$, respectively; and two angles $\alpha_F, \alpha_B$, for changing the robot configuration, which can be changed during robot operations.

<figure><img src="image_8.png" /><figcaption>Figure 3. Notations and definitions of two angles $\alpha_F$ and $\alpha_B$.</figcaption></figure>

The relationship between the resulting control vector, including force and torque elements, in the body frame, denoted as $\mathbf{F}_B$, and the force vector applied on the thrusters, denoted as $\mathbf{F}_m$, is described as a mapping through the configuration matrix, denoted as $\mathbf{A}$, which describes the geometric organization of thrusters in the body frame:

$$ \mathbf{F}_B = \mathbf{A}(\alpha_F, \alpha_B)\mathbf{F}_m = \begin{pmatrix} \mathbf{f} \\ \boldsymbol{\tau} \end{pmatrix} \qquad (1) $$

where $\mathbf{F}_B \in \mathbb{R}^6$, $\mathbf{A} \in \mathbb{R}^{6 \times m}$, $\mathbf{F}_m = [F_{m,1} \ F_{m,2} \ \dots \ F_{m,m}]^T \in \mathbb{R}^m$, and $m$ is the number of thrusters, $m = 7 > 6$. Because the system has 6 DOFs with 7 actuators, the actuation system is said to be redundant or over-actuated.
From the scheme of the robot, the configuration matrix is as follows:

$$ 
\begin{aligned}
\mathbf{A} &= \begin{pmatrix} \mathbf{u}_1^B & \mathbf{u}_2^B & \cdots & \mathbf{u}_m^B \\ \mathbf{r}_1^B \otimes \mathbf{u}_1^B & \mathbf{r}_2^B \otimes \mathbf{u}_2^B & \cdots & \mathbf{r}_m^B \otimes \mathbf{u}_m^B \end{pmatrix} \\
&= \begin{pmatrix} \mathbf{u}_1^B & \mathbf{u}_2^B & \cdots & \mathbf{u}_m^B \\ \tau_1^B & \tau_2^B & \cdots & \tau_m^B \end{pmatrix} = \begin{pmatrix} \mathbf{A}_1 \\ \mathbf{A}_2 \end{pmatrix}
\end{aligned} 
\quad (2) $$

where $m = 7$ and $\mathbf{u}_1^B, \dots, \mathbf{u}_7^B$ and $\mathbf{r}_1^B, \dots, \mathbf{r}_7^B$ are shown in Appendix A. The basic idea for the computing matrix $\mathbf{A}$ is to use transformation matrices between the coordinate systems. Because of space limitations, this computation is not shown in this paper. When two angles, $\alpha_F$ and $\alpha_B$ (Figure 3), are varied, the robot's configuration ($\mathbf{A}$ matrix) changes.

In this section, we consider an energy-efficient configuration and the control allocation problem, in which the objective function is defined using the Euclidean norm of the applied force vector, $\mathbf{F}_m$ with respect to mechanical constraints: the thruster limitations. In particular, the problem is formulated as

$$ \min_{\alpha_F, \alpha_B, \mathbf{F}_m} J = \|\mathbf{F}_m\|^2 \quad (3a) $$

$$ \text{s.t.} \quad 45^{\circ} \le \alpha_F, \alpha_B \le 90^{\circ} \quad (3b) $$

$$ \mathbf{F}_m \in \mathbb{F} \quad (3c) $$

$$ \mathbf{F}_B^d - \mathbf{A}(\alpha_F, \alpha_B)\mathbf{F}_m = 0 \quad (3d) $$

where $\mathbf{F}_B^d$ is the desired control vector (output from the controller), and $\mathbb{F}$ is a feasible set of thruster forces. The constraint (3b) is the mechanical limitation on the robot, in which two angles can vary from 45° to 90°.

The objective function is chosen as the Euclidean norm of the applied force vector. This is reasonable because of the nearly linear characteristics of the thrusters used in our robot (see more details in the simulation section). The problem objective is to find two angles, $\alpha_F, \alpha_B$ and applied force vector $\mathbf{F}_m$, to minimize function $J$ and to satisfy the constraints. This is a nonlinear optimization problem that is solved at each sampling time (called online optimization) because the desired control vector $\mathbf{F}_B^d$ is changed in each time step in the general case. Note that in our problem, the configuration matrix $\mathbf{A}$ is dynamic and has two angles, $\alpha_F$ and $\alpha_B$ (see Figure 3).

Other perspectives we needed to consider are the reactivity of the robot (the time for state propagation or system response) and the time delay of the changing configurations. If the system response is too fast, we cannot apply online optimization. Our objective was to solve the online optimization problem; therefore, we assumed that the time required for solving at least one iteration of the optimization problem is less than the time that the system needs from the current to the next state. In our case of an underwater robot, this assumption is reasonable.

For the time delay when changing configurations, assume that at time step $k$, we have two angles: $\alpha_{Fk}$ and $\alpha_{Bk}$. At the next time step, $k+1$, assume that we obtain a solution from the optimization problem with two angles $\alpha_{F(k+1)}$ and $\alpha_{B(k+1)}$. Physically, the robot requires time, $\Delta t_\alpha$, to change from $\alpha_{Fk}$ to $\alpha_{F(k+1)}$ and from $\alpha_{Bk}$ to $\alpha_{B(k+1)}$, which is the new optimal configuration. This changing time cannot be too fast, given the limitations of the DC motors used for changing the robot configuration. However, this change must be completed before the next time step, $k+1$. If not, the configuration matrix will not be associated with the correct corresponding time step $k+1$. In other words, the time needed for changing, $\Delta t_\alpha$, must be less than the sampling time. Therefore, the consecutive values of these two angles must be small enough.

To solve our problem, two assumptions were applied, as follows:

**Assumption 1.** The reactivity of the system is long enough, i.e., to solve the online optimization problem and basic mathematical operations. In particular, the sampling time of our underwater
robot is 0.1 (s). The computational time of one-iteration optimization and other basic mathematical operations is in centiseconds, e.g., 0.01 (s) (this depends on the computation system) and the system reactivity of an underwater robot, which normally depends on its shape and drag coefficients, is also in centiseconds.

**Assumption 2.** The time for changing the mechanical system between two consecutive angles is fast enough in one sampling time.

In practice, Assumption 1 is reasonable for underwater robots. Assumption 2 can be satisfied if the derivation between two consecutive angles is small enough.

**Remark 1.** The problem (3) is a nonconvex parametric optimization problem, which has to be solved at each sampling time. Thus, as obtaining exact optimal solution is too challenging or probably impossible, the idea is to find an approximate solution.

**Remark 2.** The deviation of two angles $\alpha_F, \alpha_B$ in two consecutive time steps is small enough, owing to mechanical and electrical limitations and Assumption 2.

### 3. Solution

The procedure of finding the approximate solution of the problem is divided into two steps: Predictor and corrector. In the predictor step, the problem is quickly solved (in one iteration) to obtain the approximately optimal two angles and applied force vector. However, the hard constraint (3d) cannot be easily satisfied with this applied force vector. With these two angles, we can instantly compute the configuration matrix and move to the next step, corrector. In this step, an algorithm is used to find a better applied force vector with respect to this hard constraint. The two following subsections describe some basic results from real-time model predictive control, which we used in the predictor step.

#### 3.1. Sequential Quadratic Programming (SQP)

Consider a nonlinear optimization problem (NLP):

$$ \min_{\mathbf{x}} J(\mathbf{x}) \qquad (4a) $$

$$ \text{s.t} \quad G(\mathbf{x}) = 0 \qquad (4b) $$

$$ H(\mathbf{x}) \geq 0 \qquad (4c) $$

Sequential quadratic programming (SQP) is an iterative method to find a Karush-Kuhn-Tucker (KKT) point of an NLP. In particular, starting with an initial guess $\mathbf{y}_0 = (\mathbf{x}_0, \lambda_0, \mu_0)$ (where $\mathbf{x}_0$ is a primal variable, and $\lambda_0, \mu_0$ are Lagrangian multipliers), an SQP method iterates:

$$ \mathbf{y}_{k+1} = \mathbf{y}_k + \delta_k \Delta \mathbf{y}_k \qquad (5) $$

where $\delta_k \in (0, 1]$, and

$$ \Delta \mathbf{y}_{k+1} = \begin{pmatrix} \Delta \mathbf{x}_k \\ \Delta \lambda_k \\ \Delta \mu_k \end{pmatrix} := \begin{pmatrix} \Delta \mathbf{x}_k \\ \tilde{\lambda}_k - \lambda_k \\ \tilde{\mu}_k - \mu_k \end{pmatrix} \qquad (6) $$

Equation (6) is obtained from the solution point $(\Delta \mathbf{x}_k, \tilde{\lambda}_k, \tilde{\mu}_k)$ of the following quadratic programming:

$$ \min_{\Delta \mathbf{x} \in \Omega_k} \frac{1}{2} \Delta \mathbf{x}^T \mathbf{A}_k \Delta \mathbf{x} + \nabla_x J(\mathbf{x}_k)^T \Delta \mathbf{x} \qquad (7a) $$

$$ \text{s.t} \quad G(\mathbf{x}_k) + \nabla_x G(\mathbf{x}_k)^T \Delta \mathbf{x} = 0 \qquad (7b) $$

$$ H(\mathbf{x}_k) + \nabla_x H(\mathbf{x}_k)^T \Delta \mathbf{x} \geq 0 \qquad (7c) $$
Readers can refer to [17] for more details. The choice of the step length $\delta_k$, Hessian matrix $A_k$, and set $\Omega_k \subset \mathbb{R}^{n_x}$ derives variants of existing SQP methods. If we choose $\delta_k := 1, \Omega_k := \mathbb{R}^{n_x}$ and the Hessian matrix as in Equation (8), this is a full-step exact Hessian SQP method, which is appealing and important [18]:

$$A_k := \nabla_x^2 L(\mathbf{x}_k, \lambda_k, \mu_k) \qquad (8)$$

This choice has an advantage, in that the full-step exact Hessian SQP method shows the same high-quality local convergence behavior as the Newton-Raphson method in the vicinity of a solution of the KKT system (for an illustration for cases with equality constraints, see [17]). Note that a good initial guess is required not only for the primal variable $\mathbf{x}$ but also for the multipliers $\lambda, \mu$ (for a proof, refer to [19]).

## 3.2. Parametric Nonlinear Optimization

In this section, we describe a parametric optimization problem, $P(\mathbf{t})$, in which a parameter is changed as follows:

$$\min_{\mathbf{x}} \quad J(\mathbf{x}, \mathbf{t}) \qquad (9a)$$

$$s.t \quad G(\mathbf{x}, \mathbf{t}) = 0 \qquad (9b)$$

$$H(\mathbf{x}, \mathbf{t}) \geq 0 \qquad (9c)$$

where $\mathbf{t}$ is a parametric variable. Note that this is not a time variable.

The problem (9) is equivalent to the following problem $P(\bar{\mathbf{t}})$:

$$\min_{\mathbf{x},\mathbf{t}} \quad J(\mathbf{x}, \mathbf{t}) \qquad (10a)$$

$$s.t \quad \mathbf{t} - \bar{\mathbf{t}} = 0 \qquad (10b)$$

$$G(\mathbf{x}, \mathbf{t}) = 0 \qquad (10c)$$

$$H(\mathbf{x}, \mathbf{t}) \geq 0 \qquad (10d)$$

The problem (10) is only different from (9) by reforming variable $\mathbf{t}$, which is fixed as an additional constraint $\mathbf{t} - \bar{\mathbf{t}} = 0$. This is introduced to show that the first iteration of the SQP approach of the problem is the first-order approximation of the solution manifold of this problem. The following theorem is extracted to highlight this point. Readers can refer to [18] for the proof and more details.

**Theorem 1** (First-order prediction by exact Hessian SQP [18]). Let us assume that we found a KKT point $(\mathbf{x}^*(0), \lambda^*(0), \mu^*(0))$ of problem $P(0)$ that satisfies the sufficient optimality conditions. If a full-step SQP algorithm with an exact Hessian for the solution of the problem $P(\epsilon)$, with $\epsilon > 0$ being sufficiently small, is started with this solution as an initial guess, then the nontrivial part of the first SQP step $(\Delta\mathbf{x}, \Delta\lambda, \Delta\mu)$ is identical to $\epsilon$ times the one-sided derivative of the solution manifold $(\mathbf{x}^*(\mathbf{t}), \lambda^*(\mathbf{t}), \mu^*(\mathbf{t}))$ of the problem $P(\mathbf{t})$, i.e.,

$$\lim_{\mathbf{t} \to \mathbf{t}_0, \mathbf{t}_0 > 0} \frac{1}{\mathbf{t}} \begin{pmatrix} \mathbf{x}^*(\mathbf{t}) - \mathbf{x}^*(0) \\ \lambda^*(\mathbf{t}) - \lambda^*(0) \\ \mu^*(\mathbf{t}) - \mu^*(0) \end{pmatrix} = \begin{pmatrix} \delta\mathbf{x} \\ \delta\lambda \\ \delta\mu \end{pmatrix} = \frac{1}{\epsilon} \begin{pmatrix} \Delta\mathbf{x} \\ \Delta\lambda \\ \Delta\mu \end{pmatrix} \qquad (11)$$

## 3.3. Online Optimization Observation in SQP

Following Theorem 1, an observation is that the first quadratic programming solution of a full-step exact Hessian SQP algorithm provides a good approximation of the exact solution if the algorithm is initialized in a neighborhood of this solution. In an online optimization scenario, it would probably be better to use an approximation of the first correction instead of waiting until the SQP algorithm converges. After the first SQP
iteration, we correct the solution if disturbances exist that affect the initial guess or violate the constraints; if no further disturbance occurs, the algorithm can continue to improve the outcome of the previous iterations. In our case of an energy-efficient configuration of the robot, the change in the two angles $\alpha_F, \alpha_B$ in each time step is small. Hence, from an initial guess, two angles can be chosen after one iteration of SQP. After that, we can correct the applied force vector to obtain a better solution.

### 3.4. Algorithm

#### 3.4.1. Predictor Step

This problem (3) can be rewritten as

$$ \min_{\alpha_F, \alpha_B, \mathbf{F}_m, \mathbf{F}_B} J = \|\mathbf{F}_m\|^2 \quad (12a) $$

$$ \text{s.t.} \quad 45^{\circ} \le \alpha_F, \alpha_B \le 90^{\circ} \quad (12b) $$

$$ \mathbf{F}_m \in \mathbb{F} \quad (12c) $$

$$ \mathbf{F}_B - \mathbf{A}(\alpha_F, \alpha_B)\mathbf{F}_m = 0 \quad (12d) $$

$$ \mathbf{F}_B - \mathbf{F}_B^d = 0 \quad (12e) $$

where $\mathbf{F}_B$ is a parametric variable, and $\mathbf{F}_B^d$ is its constraint and is changed during a time instant (output from a controller). This problem has the form of problem (10) and is solved in one iteration using SQP.

By denoting $\mathbf{x} = [\alpha_F \ \alpha_B \ \mathbf{F}_m]^T$, the problem (12) can be formed as

$$ \min_{\mathbf{x}, \mathbf{F}_B} J(\mathbf{x}) \quad (13a) $$

$$ \text{s.t.} \quad G(\mathbf{x}, \mathbf{F}_B) = 0 \quad (13b) $$

$$ H(\mathbf{x}) \le 0 \quad (13c) $$

$$ \mathbf{F}_B - \mathbf{F}_B^d = 0 \quad (13d) $$

where $\mathbf{F}_B$ replaces the role of $\mathbf{t}$ and $\mathbf{F}_B^d$ replaces the role of $\bar{\mathbf{t}}$ in problem $P(\bar{\mathbf{t}})$.

The quadratic programming used to find direction in SQP, $\Delta\mathbf{x}_k, \Delta\mathbf{F}_B$, is as follows:

$$ \min_{\Delta\mathbf{x}, \Delta\mathbf{F}_B} \frac{1}{2} \Delta\mathbf{x}^T \nabla_x^2 \mathcal{L} \Delta\mathbf{x} + \nabla_x J^T \Delta\mathbf{x} + \nabla_{\mathbf{F}_B} J^T \Delta\mathbf{F}_B + \frac{1}{2} \Delta\mathbf{F}_B^T \nabla_{\mathbf{F}_B}^2 \mathcal{L} \Delta\mathbf{F}_B + \Delta\mathbf{F}_B^T \nabla_{\mathbf{x}, \mathbf{F}_B}^2 \mathcal{L} \Delta\mathbf{x} \quad (14a) $$

$$ \text{s.t.} \quad G + \nabla_{\mathbf{F}_B} G^T \Delta\mathbf{F}_B + \nabla_x G^T \Delta\mathbf{x} = 0 \quad (14b) $$

$$ \mathbf{H} + \nabla_{\mathbf{F}_B} \mathbf{H}^T \Delta\mathbf{F}_B + \nabla_x \mathbf{H}^T \Delta\mathbf{x} \le 0 \quad (14c) $$

$$ \Delta\mathbf{F}_B - \Delta\mathbf{F}_B^d = 0 \quad (14d) $$

where $\mathcal{L}$ is a Lagrangian function.

The problem (14) is quadratic: it can be efficiently solved by functions in MATLAB, CPLEX, or others. Note that in the *predictor* step, only one iteration of SQP is performed.

#### 3.4.2. Corrector Step

By solving Problem (12) in the *predictor* step, we obtained a solution for two angles $\alpha_F, \alpha_B$ and the applied force vector $\mathbf{F}_m$. The two angles can be applied to the robot; however, with one iteration, $\mathbf{F}_m$ does not easily satisfy the hard constraint (12c). With two angles, we have a configuration matrix, $\mathbf{A}$; the following optimization problem is solved to find a new applied force vector $\mathbf{F}_m$:

$$ \min_{\mathbf{F}_m} J = \|W_F \mathbf{F}_m\|^2 + \|W_B (\mathbf{A} \mathbf{F}_m - \mathbf{F}_B^d)\|^2 \quad (15a) $$

$$ \text{s.t.} \quad \mathbf{F}_m \in \mathbb{F} \quad (15b) $$

where $W_F, W_B$ are weighting vectors.
The problem (15) can be considered a classical control allocation problem and be efficiently solved by optimization-based methods or pseudo-based methods [8]. The dead-zone compensation [20] is applied if necessary.

Algorithm 1 shows the procedure to solve the energy-efficient configuration and control allocation problem for our robot.

**Algorithm 1** Energy-efficient and control allocation algorithm.

**Input:** Parametric variable $\mathbf{F}_B^d$ (output from the controller)

**Output:** Local optimal angles $\alpha_F, \alpha_B$ and applied force vector $\mathbf{F}_m$

Predictor step:

1: Initial guess: primal-dual variable, $(\mathbf{x}_k, \boldsymbol{\lambda}_k, \boldsymbol{\mu}_k)$; Hessian matrix $\nabla_x^2 \mathcal{L}$, and $\nabla_x J, G, \nabla_x G, \nabla_x H, H$: all are evaluated at $(\mathbf{x}_k, \boldsymbol{\lambda}_k, \boldsymbol{\mu}_k)$.

2: Solve QP problem (14) and obtain corresponding solution $(\Delta\mathbf{x}_k, \tilde{\boldsymbol{\lambda}}_k, \tilde{\boldsymbol{\mu}}_k)$

3: Compute multipliers direction $\Delta\boldsymbol{\lambda}_k = \tilde{\boldsymbol{\lambda}}_k - \boldsymbol{\lambda}_k, \Delta\boldsymbol{\mu}_k = \tilde{\boldsymbol{\mu}}_k - \boldsymbol{\mu}_k$

4: Update $\mathbf{x}_{k+1} = \mathbf{x}_k + \Delta\mathbf{x}_k, \boldsymbol{\lambda}_{k+1} = \boldsymbol{\lambda}_k + \Delta\boldsymbol{\lambda}_k = \tilde{\boldsymbol{\lambda}}_k$, and $\boldsymbol{\mu}_{k+1} = \boldsymbol{\mu}_k + \Delta\boldsymbol{\mu}_k = \tilde{\boldsymbol{\mu}}_k \triangleright \text{This is the initial guess for the next time step.}$

5: Extract two angles $\alpha_F, \alpha_B$ from vector $\mathbf{x}_{k+1}$

6: Compute configuration matrix, $\mathbf{A}$, corresponding to two angles $\alpha_F, \alpha_B$.

Corrector step:

7: Solve problem (15) to obtain applied force vector $\mathbf{F}_m$

**Remark 3.** The difference between online optimization and real-time schemes is the initial strategy. This depends on each specific problem. However, in some systems, the initial values are fixed because of limitations or typical applications. For such systems, an offline optimization procedure may be conducted first to find a suitable initial guess. If not, the solution of the current step can be improved in comparison with the previous step. For our dynamically reconfigurable underwater robot, for instance, because of transportation and karst exploration, from the beginning of a mission, the initial configuration of the robot is chosen as $\alpha_F = \alpha_B = 45^\circ$. Then, the initial guess is $\alpha_F = \alpha_B = 45^\circ, \mathbf{F}_m = \mathbf{0}$.

# 4. Simulation Results

This section presents the simulation results of the proposed approach for our simulated robot. The control diagram of the simulation is shown in Figure 4. A PID or quaternion-based controller was used to derive desired control vector $\mathbf{F}_B^d$. The proposed algorithm inside the energy-efficient control allocation block found the applied force vector, $\mathbf{F}_m$, corresponding to each desired control vector. The inputs of the thrusters (PWM) were interpolated from vector $\mathbf{F}_m$ by using inverse thruster characteristics. The next subsection presents more details about the simulated robot and thruster characteristics.

<figure><img src="image_9.png" /><figcaption>**Figure 4.** Control loop simulation.</figcaption></figure>

## 4.1. Simulated Robot

The simulated robot we built is shown in Figure 5a, which is the same as our prototype robot. Note that in the simulations, external disturbances and model uncertainties were not considered. The robot has three forward thrusters and four backward thrusters. The two blue cylinders are waterproof tubes containing electronic boards, and the two green cylinders are battery tubes. The thruster characteristics are shown in Figure 5b, which
approximates the T200 thruster of BlueRobotics [21]. The saturation values of the thrusters are 1100 µs and 1900 µs. The dead zone of the thrusters is [1475 µs – 1525 µs]. The robot can vary its configuration. The dynamic model of the robot is simplified as Equation (16). Assume that all feedback states are completely estimated:

$$F_u = m_u \dot{u} - d_u u \qquad (16a)$$

$$F_v = m_v \dot{v} - d_v v \qquad (16b)$$

$$F_w = m_w \dot{w} - d_w w \qquad (16c)$$

$$\Gamma_p = m_p \dot{p} - d_p p \qquad (16d)$$

$$\Gamma_q = m_q \dot{q} - d_q q \qquad (16e)$$

$$\Gamma_r = m_r \dot{r} - d_r r \qquad (16f)$$

where $m_u, m_v, m_w, m_p, m_q, m_r$ are the total masses (dry mass+added mass or inertia) along each motion axis; $d_u, d_v, d_w, d_p, d_q, d_r$ are the quadratic damping terms for each motion axis. Note that all coupling terms are neglected. Because the weight of our prototype robot in water is approximately 15 kg, the dynamic parameters of the robot are chosen as $m_u = m_v = m_w = 15$ kg, $m_p = m_q = m_r = 1$ kg, $d_u = d_v = d_w = d_p = d_q = d_r = 1$ kg.

<figure><img src="image_10.png" /><figcaption>Figure 5. Simulated robot and thruster characteristics. (a) Simulated robot, unit of three axis is meter. (b) Thruster characteristics.</figcaption></figure>

In general, the robot operates with a control loop, in which a controller derives a desired control vector $F_B^d$. In this section, we describe our simulations of the problem (3) when $F_B^d$ is the dynamic parameter. We simulated two missions—path-following and station-keeping (observation) problems—which are important in underwater robotics.

## 4.2. Path-Following Problem

For the path-following problem, a line of sight (LoS)-based guidance method [22] was used in this simulation. We compared the energy-like criterion between the two static configurations and the dynamic one in this mission. A PID controller was used in this simulation. The chosen path is a spatial ellipse, which is parameterized as follows:

$$x = 60 \cos(0.2618t) \qquad (17)$$

$$y = 60 \sin(0.2618t) \qquad (18)$$

$$z = \sin(0.2618t) + 5 \qquad (19)$$

where $t$ is a path parameter.

The desired composite speed is $U_d = 2m/s$. The initial posture of the robot is $[x(0) \ y(0) \ z(0) \ \phi(0) \ \theta(0) \ \psi(0)]^T = [64(m) \ 3(m) \ 0 \ 0 \ 0 \ 3\pi/4]^T$. The initial speed of the AUV is $[u(0) \ v(0) \ w(0) \ p(0) \ q(0) \ r(0)]^T = [1.5(m/s) \ 0 \ 0 \ 0 \ 0 \ 0]^T$.
To evaluate the efficiency of our approach (dynamic configuration) in comparison with that of others (static configurations), we simulated the path-following problem for the three cases described in Table 1.

<table><caption>**Table 1.** Simulation cases for path-following problem.</caption><thead><tr><td>No. Case</td><td>Two Angles α<sub>F</sub>, α<sub>B</sub></td><td>Notes</td></tr></thead><tbody><tr><td>1</td><td>α<sub>F</sub> = α<sub>B</sub> = 70°</td><td>Simulation results in Figure 6</td></tr><tr><td>2</td><td>α<sub>F</sub> = α<sub>B</sub> = 90°</td><td>Simulation results in Figure 7</td></tr><tr><td>3</td><td>dynamic</td><td>Simulation results in Figure 8</td></tr></tbody></table>

<figure><img src="image_11.png" /><figcaption>**Figure 6.** Path following for ellipse with configuration ($α_F = α_B = 70°$). (a) Trajectory of robot. (b) PWM of 7 thrusters.</figcaption></figure>

<figure><img src="image_12.png" /><figcaption>**Figure 7.** Path following for ellipse with configuration ($α_F = α_B = 90°$). (a) Trajectory of robot. (b) PWM of 7 thrusters.</figcaption></figure>

<figure><img src="image_13.png" /><figcaption>**Figure 8.** *Cont.*</figcaption></figure>
<figure><img src="image_14.png" /><figcaption>**Figure 8.** Path following for ellipse with dynamic configuration. (a) Trajectory of robot. (b) PWM of 7 thrusters. (c) Evolution of 2 angles.</figcaption></figure>

The energy-like criterion evolutions of the path-following problem for the three simulation cases are shown in Figure 9. We found that the path-following performance was guaranteed for all three cases (see Figures 6a, 7a and 8a). However, with the dynamic configuration, from the energy perspective, the dynamic configuration showed better performance than the other two (see Figure 9). Note that this is only a local optimal solution, and in the configuration space, another optimal solution could exist. For each specific mission, from the initial values ($α_F = α_B = 45°$), the robot configuration converges to a local optimal solution. If the mission is suddenly changed, this means that the desired control vector is largely disturbed. This will be carefully investigated and could be an interesting future work, so it is not mentioned in this paper. Following Figure 9, we have two instances at which the energy consumption of the dynamic configuration is larger than the fixed configuration. This happened because some disturbances were injected into the controller to investigate the response of the system with respect to the uncertainties, although this is out of the scope of this paper and will be in the future research. In particular, for the first instance, from 23.4(s) to 27.6(s), a small disturbance was injected into the controller, and for the second one, from 46.8(s) to 46.9(s), a very large disturbance was injected into the controller in a short time. With a small disturbance, the performance of the path-following problem was guaranteed. However, with a very large disturbance, following a path was not guaranteed (a peak in Figure 8a). Indeed, Theorem 1 was violated in this case.

<figure><img src="image_15.png" /><figcaption>**Figure 9.** Energy-like criteria for path-following problem.</figcaption></figure>

### 4.3. Station-Keeping (Observation) Problem

For the station-keeping (observation) problem, the robot normally has to rotate about some DOFs and maintain a constant position, e.g., constant depth. This probably could not be achieved by an under-actuated system, which has some uncontrollable DOFs. Owing to our robot's versatility, the dynamically reconfigurable robot can easily perform this mission. In this part, we present the simulation results of the observation problem with our robot, in which the robot dove to the desired depth and then rotated with the desired angular velocities, i.e., $[x^d y^d z^d](m)^T = [0 \ 0 \ 1]^T$ and $[p^d q^d r^d](rad/s)^T = [1 \ 1 \ 1]^T$.
The controller was designed with quaternion techniques to avoid singularities (gimbal lock) [23]. The simulations included fixed and dynamic configurations. The simulation results of the fixed configurations, in which $\alpha_F = \alpha_B = 90^\circ$, are shown in Figure 10. The simulation results of the dynamic configurations are depicted in Figure 11. Note that in the simulation, we assumed that all states of the robot could be completely measured or estimated.

<figure><img src="image_16.png" /><figcaption>Figure 10. Simulation results with fixed configuration. (a) Positions of robot. (b) Angular velocities. (c) Robot trajectory. (d) PWM evolution of 7 thrusters.</figcaption></figure>

<figure><img src="image_17.png" /><figcaption>Figure 11. Cont.</figcaption></figure>
<figure><img src="image_18.png" /><figcaption>**Figure 11.** Simulation results with dynamic configuration. (a) Positions of robot. (b) Angular velocities. (c) Robot trajectory. (d) PWM evolution of 7 thrusters. (e) Two angles' evolution: $\alpha_F$-blue; $\alpha_B$-red.</figcaption></figure>

The control performance was guaranteed in both the fixed and dynamic configurations. The robot reached the desired depth (Figures 10a and 11a) and followed the desired angular velocities (Figures 10b and 11b). As shown in Figures 10d and 11d, the energy-like criterion of fixed configuration was better than that of dynamic configuration because the two angles $\alpha_F = \alpha_B = 90^\circ$ could be considered a local optimal solution in this mission (mainly the rotation task). The two angles' evolution is shown in Figure 11e, which converge to $90^\circ$, the same as in the fixed configuration. With initial value $\alpha_F = \alpha_B = 45^\circ$, the robot's configuration continues to improve through the samples. Following Figure 11 (observation problem), the normally rotating priority task is considered. A locally optimal fixed configuration (rotating priority) is chosen. The dynamic configuration converges to the chosen optimal fixed configuration through the time. This shows that our approach drove the system to the locally optimal configuration.

**Remark 4.** The proposed method can be used to find the local energy-efficient configuration of a dynamically reconfigurable robot, as described in the previous sections, for problems in which the change in the parametric variable is small enough. However, missions requiring a large change in the parametric variable (the desired control vector) or one DOF to vary from uncontrollable to controllable or vice versa during configuration changes are outside the scope of the proposed method and will be a future research area in terms of system stability and controller design considering a switching mechanism between controllable and uncontrollable DOFs. The experiment we next describe highlights this remark.

## 5. Experiment Results

The real robot was tested in a swimming pool with different configurations. In this test, the robot performed four tasks with two corresponding angles: travel straight (from point A to point B, $\alpha_F = \alpha_B = 45^\circ$), complete a turn 180° (around point B, $\alpha_F = 85^\circ, \alpha_B = 45^\circ$), dive to a predefined depth (from point B to point C, $\alpha_F = 85^\circ, \alpha_B = 85^\circ$), and perform a sway (from point C to point D, $\alpha_F = 85^\circ, \alpha_B = 85^\circ$). The robot trajectory is illustrated in Figure 12a. The values of the two angles ($\alpha_F, \alpha_B$) for each trajectory are depicted in Figure 12d. The desired control vector (output from the PID controller), including the desired force and torque elements, is shown in Figure 12b,c. Thanks to our robot, which can vary its configuration by changing two angles $\alpha_F, \alpha_B$, we can divide the feasible space of these two angles into four regions (Figure 13), in which the robot has priority in its DOFs: *surge priority*, *sway/heave priority*, and *rotating priority*. The term *priority* means that the robot prefers to use such a task in the priority region. For our experiment, the two angles converged to the corresponding priority region.
<figure><img src="image_19.png" /><figcaption>**Figure 12.** Experiment results. (a) Experiment descriptions. (b) Desired forces expressed in body frame. (c) Desired torques expressed in body frame. (d) Evolution of two angles.</figcaption></figure>

<figure><img src="image_20.png" /><figcaption>**Figure 13.** Two angles in feasible map and priority regions.</figcaption></figure>

When we applied the proposed method to this trajectory, with initial values $α_F = α_B = 45°$, the trajectory was reasonable and optimal from point A to point B and when turning around point B. Nevertheless, to directly dive from point B to point C, the robot had to considerably change its configuration enough because the heave DOF is uncontrollable with $α_F = α_B = 45°$ and the proximity of these angle values. As such, the robot could not directly perform this dive. So, the robot can use a *hybrid mechanism* between dynamic and static states during its operations. For some parts of the trajectory, the robot can use the dynamic mechanism and, for others, the robot can use a static one with time spent to change the configuration.

## 6. Conclusions and Future Studies

In this paper, we proposed an approach for an energy-efficient configuration and the control allocation of a dynamically reconfigurable underwater robot that was built for karst or confined environment exploration. The energy-like criterion was minimized with respect to the robot constraints. The proposed method was solved online (for each sampling time), corresponding to the robot's dynamic configuration. The approach was
divided into two steps: a *predictor* step and a *corrector* step. In the *predictor* step, the solution of one iteration SQP was chosen for the robot configuration. In the *corrector* step, quadratic programming, as a classical CA method, was solved to adjust the applied force vector that could be assigned for the actuators. The simulation results showed the efficiency of the proposed method through the application of two problems: path following and station keeping (observation). In the future, we will perform real tests with this method. Moreover, external disturbances and model uncertainties will be considered in our subsequent studies.

**Author Contributions:** Conceptualization, T.D., L.L., R.Z.; methodology, T.D., L.L., R.Z. and B.R.; software, T.D., L.L. and R.Z.; validation, T.D., L.L. and R.Z.; writing—original draft preparation, T.D.; writing—review and editing, L.L.; supervision, L.L. and R.Z.; project administration, L.L.; funding acquisition, L.L. All authors have read and agreed to the published version of the manuscript.

**Funding:** This project was supported by the LabEx NUMEV (ANR-10-LABX-0020) within the I-SITE MUSE (ANR-16-IDEX-0006) and the Region Occitanie (french FEDER funds).

**Acknowledgments:** The authors would like to thank Numev Labex, MUSE, Montpellier University; Region Occitanie; and FEDER for supporting this study.

**Conflicts of Interest:** The authors declare no conflict of interest.

## Abbreviations

The following abbreviations are used in this manuscript:

* AUV Autonomous Underwater Robot

* SQP Sequential Quadratic Programming

* PWM Pulse Width Modulation

* DOF Degree of Freedom

* MPC Model Predictive Control

* CA Control Allocation

* PID Proportional–Integral–Derivative controller

## Appendix A. Configuration Matrix

The elements of the configuration matrix **A** are presented in Table A1. Note that $d$ and $d_i, i \in \{F, e, t\}$ are geometrical distances corresponding to the shape of the robot. $\beta_i, i \in \{1, 2, ..., 7\}$ is an angle.

<table><caption>**Table A1.** Elements of **a** matrix.</caption><tr><td><b>u</b><sup>B</sup><sub>1</sub> = <table style="display: inline-block; vertical-align: middle; text-align: center;"><tbody><tr><td style="border-bottom: 1px solid black; padding: 0 5px;"><sup>1</sup>&frasl;<sub>&radic;2</sub>(sin&alpha;<sub>F</sub> + cos&alpha;<sub>F</sub>)</td></tr><tr><td style="padding: 0 5px;"><sup>&radic;3</sup>&frasl;<sub>2&radic;2</sub>(-cos&alpha;<sub>F</sub> + sin&alpha;<sub>F</sub>)</td></tr><tr><td style="border-bottom: 1px solid black; padding: 0 5px;"><sup>1</sup>&frasl;<sub>2&radic;2</sub>(-cos&alpha;<sub>F</sub> + sin&alpha;<sub>F</sub>)</td></tr></tbody></table></td><td><b>r</b><sup>B</sup><sub>1</sub> = <table style="display: inline-block; vertical-align: middle; text-align: center;"><tbody><tr><td style="border-bottom: 1px solid black; padding: 0 5px;">(d<sub>F</sub> + L<sub>F</sub>) - &radic;<span style="text-decoration: overline;">d<sub>e</sub><sup>2</sup> + d<sup>2</sup></span>.cos(&alpha;<sub>F</sub> + &beta;<sub>1</sub>)</td></tr><tr><td style="padding: 0 5px;">- <sup>d<sub>t</sub></sup>&frasl;<sub>2</sub> - &radic;<span style="text-decoration: overline;">3(d<sub>e</sub><sup>2</sup> + d<sup>2</sup>)sin(&alpha;<sub>F</sub> + &beta;<sub>1</sub>)</span></td></tr><tr><td style="border-bottom: 1px solid black; padding: 0 5px;"><sup>&radic;3</sup>&frasl;<sub>2</sub>d<sub>t</sub> - &radic;<span style="text-decoration: overline;">d<sub>e</sub><sup>2</sup> + d<sup>2</sup>sin(&alpha;<sub>F</sub> + &beta;<sub>1</sub>)</span></td></tr></tbody></table></td></tr><tr><td><b>u</b><sup>B</sup><sub>2</sub> = <table style="display: inline-block; vertical-align: middle; text-align: center;"><tbody><tr><td style="border-bottom: 1px solid black; padding: 0 5px;"><sup>&radic;2</sup>&frasl;<sub>0</sub>(cos&alpha;<sub>F</sub> + sin&alpha;<sub>F</sub>)</td></tr><tr><td style="padding: 0 5px;"><sup>&radic;2</sup>&frasl;<sub>0</sub>(cos&alpha;<sub>F</sub> - sin&alpha;<sub>F</sub>)</td></tr><tr><td style="border-bottom: 1px solid black; padding: 0 5px;"><sup>&radic;2</sup>&frasl;<sub>0</sub>(cos&alpha;<sub>F</sub> - sin&alpha;<sub>F</sub>)</td></tr></tbody></table></td><td><b>r</b><sup>B</sup><sub>2</sub> = <table style="display: inline-block; vertical-align: middle; text-align: center;"><tbody><tr><td style="border-bottom: 1px solid black; padding: 0 5px;">(d<sub>F</sub> + L<sub>F</sub>) - &radic;<span style="text-decoration: overline;">d<sub>e</sub><sup>2</sup> + d<sup>2</sup></span>.cos(&alpha;<sub>F</sub> + &beta;<sub>2</sub>)</td></tr><tr><td style="padding: 0 5px;">d<sub>t</sub></td></tr><tr><td style="border-bottom: 1px solid black; padding: 0 5px;"><sup>&radic;2</sup>&frasl;<sub>0</sub>(d<sub>t</sub><sup>2</sup> + d<sup>2</sup>sin(&alpha;<sub>F</sub> + &beta;<sub>2</sub>)</td></tr></tbody></table></td></tr><tr><td><b>u</b><sup>B</sup><sub>3</sub> = <table style="display: inline-block; vertical-align: middle; text-align: center;"><tbody><tr><td style="border-bottom: 1px solid black; padding: 0 5px;"><sup>1</sup>&frasl;<sub>&radic;2</sub>(sin&alpha;<sub>F</sub> + cos&alpha;<sub>F</sub>)</td></tr><tr><td style="padding: 0 5px;"><sup>&radic;3</sup>&frasl;<sub>2&radic;2</sub>(cos&alpha;<sub>F</sub> - sin&alpha;<sub>F</sub>)</td></tr><tr><td style="border-bottom: 1px solid black; padding: 0 5px;"><sup>1</sup>&frasl;<sub>2&radic;2</sub>(sin&alpha;<sub>F</sub> - cos&alpha;<sub>F</sub>)</td></tr></tbody></table></td><td><b>r</b><sup>B</sup><sub>3</sub> = <table style="display: inline-block; vertical-align: middle; text-align: center;"><tbody><tr><td style="border-bottom: 1px solid black; padding: 0 5px;">(d<sub>F</sub> + L<sub>F</sub>) - &radic;<span style="text-decoration: overline;">d<sub>e</sub><sup>2</sup> + d<sup>2</sup></span>.cos(&alpha;<sub>F</sub> + &beta;<sub>3</sub>)</td></tr><tr><td style="padding: 0 5px;"><sup>d<sub>t</sub></sup>&frasl;<sub>2</sub> + &radic;<span style="text-decoration: overline;">3(d<sub>e</sub><sup>2</sup> + d<sup>2</sup>)</span>.sin(&alpha;<sub>F</sub> + &beta;<sub>3</sub>)</td></tr><tr><td style="border-bottom: 1px solid black; padding: 0 5px;"><sup>&radic;3</sup>&frasl;<sub>2</sub>d<sub>t</sub> - &radic;<span style="text-decoration: overline;">d<sub>e</sub><sup>2</sup> + d<sup>2</sup></span>.sin(&alpha;<sub>F</sub> + &beta;<sub>3</sub>)</td></tr></tbody></table></td></tr><tr><td><b>u</b><sup>B</sup><sub>4</sub> = <table style="display: inline-block; vertical-align: middle; text-align: center;"><tbody><tr><td style="border-bottom: 1px solid black; padding: 0 5px;"><sup>&radic;2</sup>&frasl;<sub>0</sub>(cos&alpha;<sub>B</sub> + sin&alpha;<sub>B</sub>)</td></tr><tr><td style="padding: 0 5px;"><sup>&radic;2</sup>&frasl;<sub>0</sub>(-cos&alpha;<sub>B</sub> + sin&alpha;<sub>B</sub>)</td></tr><tr><td style="border-bottom: 1px solid black; padding: 0 5px;"><sup>&radic;2</sup>&frasl;<sub>0</sub>(cos&alpha;<sub>B</sub> - sin&alpha;<sub>B</sub>)</td></tr></tbody></table></td><td><b>r</b><sup>B</sup><sub>4</sub> = <table style="display: inline-block; vertical-align: middle; text-align: center;"><tbody><tr><td style="border-bottom: 1px solid black; padding: 0 5px;">-(d<sub>F</sub> + &radic;<span style="text-decoration: overline;">d<sub>e</sub><sup>2</sup> + d<sup>2</sup></span>.cos(&alpha;<sub>B</sub> + &beta;<sub>4</sub>))</td></tr><tr><td style="padding: 0 5px;">-(&radic;<span style="text-decoration: overline;">d<sub>e</sub><sup>2</sup> + d<sup>2</sup></span>.sin(&alpha;<sub>B</sub> + &beta;<sub>4</sub>))</td></tr><tr><td style="border-bottom: 1px solid black; padding: 0 5px;"><sup>-dt</sup>&frasl;<sub>2</sub></td></tr></tbody></table></td></tr><tr><td><b>u</b><sup>B</sup><sub>5</sub> = <table style="display: inline-block; vertical-align: middle; text-align: center;"><tbody><tr><td style="border-bottom: 1px solid black; padding: 0 5px;"><sup>&radic;2</sup>&frasl;<sub>0</sub>(cos&alpha;<sub>B</sub> + sin&alpha;<sub>B</sub>)</td></tr><tr><td style="padding: 0 5px;"><sup>&radic;2</sup>&frasl;<sub>0</sub>(cos&alpha;<sub>B</sub> - sin&alpha;<sub>B</sub>)</td></tr><tr><td style="border-bottom: 1px solid black; padding: 0 5px;"><sup>&radic;2</sup>&frasl;<sub>0</sub>(cos&alpha;<sub>B</sub> - sin&alpha;<sub>B</sub>)</td></tr></tbody></table></td><td><b>r</b><sup>B</sup><sub>5</sub> = <table style="display: inline-block; vertical-align: middle; text-align: center;"><tbody><tr><td style="border-bottom: 1px solid black; padding: 0 5px;">-(d<sub>F</sub> + &radic;<span style="text-decoration: overline;">d<sub>e</sub><sup>2</sup> + d<sup>2</sup></span>.cos(&alpha;<sub>B</sub> + &beta;<sub>5</sub>))</td></tr><tr><td style="padding: 0 5px;">d<sub>t</sub></td></tr><tr><td style="border-bottom: 1px solid black; padding: 0 5px;"><sup>&radic;2</sup>&frasl;<sub>0</sub>(d<sub>t</sub><sup>2</sup> + d<sup>2</sup>.sin(&alpha;<sub>B</sub> + &beta;<sub>5</sub>))</td></tr></tbody></table></td></tr></table>
<table><caption>**Table A1.** *Cont.*</caption><tr><td><b>u</b><sub>6</sub><sup>B</sup> = (<sup>cosα<sub>B</sub> + sinα<sub>B</sub></sup>&frasl;<sub>√2</sub><br>cosα<sub>B</sub> - sinα<sub>B</sub><br>0</sub>)</td><td><b>r</b><sub>6</sub><sup>B</sup> = (<sup>- (d<sub>F</sub> + √d<sub>F</sub><sup>2</sup> + d<sub>ε</sub><sup>2</sup>cos(α<sub>B</sub> + β<sub>6</sub>))</sup>&frasl;<sub>√d<sub>F</sub><sup>2</sup> + d<sub>ε</sub><sup>2</sup>sin(α<sub>B</sub> + β<sub>6</sub>)</sub><br>-d<sub>t</sub>)</td></tr><tr><td><b>u</b><sub>7</sub><sup>B</sup> = (<sup>cosα<sub>B</sub> + sinα<sub>B</sub></sup>&frasl;<sub>√2</sub><br>0<br>-cosα<sub>B</sub> + sinα<sub>B</sub><br>√2</sub>)</td><td><b>r</b><sub>7</sub><sup>B</sup> = (<sup>- (d<sub>F</sub> + √d<sub>F</sub><sup>2</sup> + d<sub>ε</sub><sup>2</sup>cos(α<sub>B</sub> + β<sub>7</sub>))</sup>&frasl;<sub>-d<sub>t</sub></sub><br>- (√d<sub>F</sub><sup>2</sup> + d<sub>ε</sub><sup>2</sup>sin(α<sub>B</sub> + β<sub>7</sub>))</sub>)</td></tr></table>

## Appendix B. Notations

This section describes the notations used in the paper. However, further notations are introduced when needed.

<table><tr><td><b>A</b></td><td>Configuration matrix</td></tr><tr><td><b>u</b><sub>i</sub><sup>B</sup></td><td>(3 × 1) unit vector of direction of the <i>i</i>th thruster with respect to body frame</td></tr><tr><td><b>r</b><sub>i</sub><sup>B</sup></td><td>(3 × 1) unit vector of position of the <i>i</i>th thruster with respect to body frame</td></tr><tr><td><b>F</b><sub>m</sub></td><td>(<i>m</i> × 1) applied force vector of <i>m</i> thrusters</td></tr><tr><td><b>F</b><sub>m,i</sub></td><td>Applied force magnitude of the <i>i</i>th thruster</td></tr><tr><td><b>F</b><sub>B</sub><sup>d</sup></td><td>(6 × 1) desired control vector (including force and torque) with respect to body frame</td></tr><tr><td><b>F</b><sub>B</sub> = (<b>f</b><sub>τ</sub>)</td><td>(6 × 1) resulting control vector (including force and torque) with respect to body frame</td></tr><tr><td>⊗</td><td>Cross product</td></tr><tr><td>||·||</td><td>Euclidean norm</td></tr><tr><td><i>m</i></td><td>Number of thrusters</td></tr><tr><td><i>n</i></td><td>Number of degrees of freedom (DOFs)</td></tr></table>

## References

1. Yim, M.; Shen, W.M.; Salemi, B.; Rus, D.; Moll, M.; Lipson, H.; Klavins, E.; Chirikjian, G.S. Modular self-reconfigurable robot systems [grand challenges of robotics]. *IEEE Robot. Autom. Mag.* **2007**, *14*, 43–52. [CrossRef]

2. Liu, J.; Zhang, X.; Hao, G. Survey on research and development of reconfigurable modular robots. *Adv. Mech. Eng.* **2016**, *8*, 1687814016659597. [CrossRef]

3. Dang, T.; Lapierre, L.; Zapata, R.; Ropars, B.; Gourmelen, G. A Dynamically Reconfigurable Autonomous Underwater Robot for Karst Exploration: Design and Experiment. *Sensors* **2022**, *22*, 3379. [CrossRef] [PubMed]

4. Page, A.B.; Steinberg, M.L. A closed-loop comparison of control allocation methods. In Proceedings of the AIAA Guidance, Navigation, and Control Conference, Dever, CO, USA, 14–17 August 2000; pp. 1760–1770.

5. Bodson, M. Evaluation of optimization methods for control allocation. *J. Guid. Control Dyn.* **2002**, *25*, 703–711 [CrossRef]

6. Fossen, T.I.; Johansen, T.A. A Survey of Control Allocation Methods for Ships and Underwater Vehicles. In Proceedings of the 2006 14th Mediterranean Conference on Control and Automation, Ancona, Italy, 28–30 June 2006; pp. 1–6.

7. Fossen, T.I.; Johansen, T.A.; Perez, T. *A Survey of Control Allocation Methods for Underwater Vehicles*; INTECH Open Access Publisher: London, UK, 2009.

8. Johansen, T.A.; Fossen, T.I. Control allocation—A survey. *Automatica* **2013**, *49*, 1087–1103. [CrossRef]

9. Skulstad, R.; Li, G.; Zhang, H.; Fossen, T.I. A Neural Network Approach to Control Allocation of Ships for Dynamic Positioning. *IFAC* **2018**, *51*, 128–133. [CrossRef]

10. Kang, J.; Choi, K. Development of an Artificial Neural Network Control Allocation Algorithm for Small Tailless Aircraft Based on Dynamic Allocation Method. *Int. J. Aeronaut. Space Sci.* **2022**, *23*, 363–378. [CrossRef]

11. Johansen, T.A.; Fossen, T.I.; Berge, S.P. Constrained nonlinear control allocation with singularity avoidance using sequential quadratic programming. *IEEE Trans. Control Syst. Technol.* **2004**, *12*, 211–216. [CrossRef]

12. Härkegård, O. Backstepping and Control Allocation with Applications to Flight Control. Ph.D. Thesis, Linköpings Universitet, Linköping, Sweden, 2003.

13. Casavola, A.; Garone, E. Fault-tolerant adaptive control allocation schemes for overactuated systems. *Int. J. Robust Nonlinear Control* **2010**, *20*, 1958–1980. [CrossRef]

14. Tohidi, S.S.; Yildiz, Y.; Kolmanovsky, I. Fault tolerant control for over-actuated systems: An adaptive correction approach. In Proceedings of the 2016 American Control Conference (ACC), Boston, MA, USA, 6–8 July 2016; pp. 2530–2535.

15. Cristofaro, A.; Polycarpou, M.M.; Johansen, T.A. Fault-Tolerant Control Allocation for Overactuated Nonlinear Systems. *Asian J. Control* **2018**, *20*, 621–634. [CrossRef]

16. Diehl, M.; Bock, H.G.; Schlöder, J.P. A real-time iteration scheme for nonlinear optimization in optimal feedback control. *SIAM J. Control Optim.* **2005**, *43*, 1714–1736. [CrossRef]
17. Nocedal, J.; Wright, S. *Numerical Optimization*; Springer Science & Business Media: New York, NY, USA, 2006.

18. Diehl, M. Real-Time Optimization for Large Scale Nonlinear Processes. Ph.D. Thesis, Heidelberg University, Heidelberg, Germany, 2001.

19. Fletcher, R. *Practical Methods of Optimization*; John Wiley & Sons: Hoboken, NJ, USA, 2013.

20. Ropars, B.; Lasbouygues, A.; Lapierre, L.; Andreu, D. Thruster's dead-zones compensation for the actuation system of an underwater vehicle. In Proceedings of the Control Conference (ECC), 2015 European, Linz, Austria, 15-17 July 2015; pp. 741-746.

21. BlueRobotics. Available online: https://bluerobotics.com/ (accessed on 4 March 2021).

22. Breivik, M.; Fossen, T.I. A unified control concept for autonomous underwater vehicles. In Proceedings of the 2006 American Control Conference, Minneapolis, MN, USA, 14-16 June 2006; pp. 7-pp.

23. Louis, S.; Lapierre, L.; Godary-Dejean, K.; Onmek, Y.; Claverie, T.; Villéger, S. *，请问 you 读一下，This is about a control problem for a robotic observation of Marine Diversity*, OCEANS: Aberdeen, UK, 2017.

**Disclaimer/Publisher's Note:** The statements, opinions and data contained in all publications are solely those of the individual author(s) and contributor(s) and not of MDPI and/or the editor(s). MDPI and/or the editor(s) disclaim responsibility for any injury to people or property resulting from any ideas, methods, instructions or products referred to in the content.