Design and Implementation of a Model Predictive Formation Tracking Control System for Underwater Multiple Small Spherical Robots

Abstract

Due to the characteristics of good concealment ability and strong mobility, multiple, small spherical underwater robot formations play an important role in near coast defense missions, such as cruising, reconnaissance, surveillance, and sensitive target capturing. Referring to the formation problem for underwater small spherical robots with limited energy, perception, and computation abilities, a trajectory tracking-based formation strategy that transforms the complex formation tracking problem into a simple trajectory tracking problem of a single robot is provided. Two layers are designed in the formation tracking strategy. The upper layer is a virtual structure-based formation algorithm. The bottom layer is a tracking controller based on model predictive control (MPC). The formation algorithm is in charge of calculating reference trajectory for each robot in the formation according to the global formation path. The MPC-based dynamic controller for each robot is designed to track the self reference trajectory. Compared with the model predictive control method used for the traditional trajectory tracking problem of a single robot, this paper additionally considers the formation constraints and the internal collision avoidance. In addition, the extended state observer (ESO) is utilized to estimate the lumped disturbance composed of environment disturbance and the inaccurate dynamic model of a small spherical robot. Not only are the numerical simulations based on MATLAB v.2015a, but physical simulations based on self-building multi-spherical robot formation platform are also carried out. Furthermore, through using two small spherical robots, a formation tracing experiment is conducted. All of the results prove that the proposed formation method is feasible and practical for small spherical robots.

1. Introduction

Autonomous underwater vehicles attract significant attention due to their great potential. These underwater vehicles could be applied to many underwater tasks, such as coastal defense. These coastal defense activities include cruising, reconnaissance, surveillance, etc., and they are key to safeguarding maritime rights and interests. However, when facing coastal defense tasks, large-scale underwater vehicles are limited in some applications due to their large size, high energy consumption, and poor maneuverability. In recent years, driven by the rapid development and integration of disciplines such as bio-science, mechanical manufacturing, materials science, control science, and micro-electronic-mechanical technology, small bio-mimetic underwater robots have gradually become important tools for defending coastal security due to their good concealment ability, strong mobility, and strong environmental adaptability. Compared with a single, small underwater robot, a coordinated group is more capable and flexible in performing complicated, large-scale, high-efficiency underwater cruising missions. The research on multiple-vehicle systems has been receiving increasing attention from engineers and scientists in recent decades [1,2,3]. One core problem concerned with the multi-small underwater robot system is formation tracking control, which requires the robots to keep a prescribed formation while simultaneously tracking a given reference trajectory [4]. Existing formation control strategies can be divided into the leader–follower method [5] and the virtual structure method [6]. The leader–follower formation strategy is a relatively common and practical method, where a single or multiple underwater robots in the formation are first determined as the leaders, and the other robots are determined as followers. The followers only need to track the leader and ensure the normal operation of the formation by keeping a certain distance and direction with respect to the leader. The key to the leader–follower method is whether the followers can acquire the information of the leader in real time and accurately track the leader behavior. It being relatively easy to design and implement are the advantages of this method. However, the followers rely on the leaders excessively, and they lack the ability to deliver feedback to the leaders, which leads to poor reliability. In addition, due to the lack of interaction between the followers, internal collision problems are prone to occur. The virtual structure-based formation strategy was proposed by Tan [7], in which the entire formation is considered as a single rigid body. The virtual structure passes completely through the formation path, and then the corresponding trajectory of each structure point is obtained. Each robot tracks the trajectory provided by the virtual structure point to complete the formation. The virtual structure is able to reduce the difficulty and workload of task allocation, and, at the same time, it can guarantee high formation accuracy. For the above two strategies, it is important that an effective controller is designed for achieving a formation tracking effect. Tao Yan et al. combined consensus with backstepping to address the problem of formation tracking for multiple AUVs [8]. Jinqiang Wang et al. studied the leader–follower formation control problem of AUVs under uncertain dynamics and ocean disturbances, and they proposed a combined tracking controller based on backstepping, adaptive neural networks, and dynamic sliding mode control techniques [9]. In reference [10], the researchers adopted the leader–follower method to obtain the reference trajectory of the followers and designed a tracking controller based on the adaptive neural network method for followers. Coordinated transportation tasks of distributed multiple AUVs were implemented based on the virtual structure strategy under weak underwater communication conditions and the nonlinear model predictive controller was designed for tracking [11,12]. Reference [13] proposed a formation control method that combines ESO with distributed model predictive control (DMPC). In response to the problem of a multiple AUV formation in environments with interference, Chao Shen also adopted a method that combined the ESO with the DMPC [4]. However, unlike reference [13], Chao Shen et al. directly took the state of the robot observed by the ESO as the input of the DMPC and proposed a Lyapunov-based model predictive control to ensure the feasibility and stability of the algorithm.

According to above studies, regarding the tracking control, plenty of controllers can be designed using the backstepping control, the sliding model control (SMC), the adaptive neural network control, and the model predictive control (MPC). The backstepping algorithm adopts a reverse reasoning approach. In a hypothetical virtual system, the algorithm links the changes in the robot state at each sampling time and the adaptive adjustment function of uncertain parameters to the designed Lyapunov function, as well as gradually corrects the deviation between the current state and the expected state, thus achieving tracking control. The sliding mode control is also a very popular algorithm for trajectory tracking due to it not only possessing the trait of model independence, but that it also has the trait of good anti-interference performance. However, the control quantity obtained by the controller has a high-frequency chattering phenomenon, which is not conducive to the execution of actuators. The neural network algorithm is another commonly utilized algorithm in underwater tracking control [14,15]. The neutral network-based tracking control is insensitive to the dynamic model of the robot, and it owns strong adaptability and learning capability. However, due to changeable underwater environments, the training and learning process are difficult, which leads to a poor real-time performance in the tracking algorithm [16,17]. Trajectory tracking controllers based on a backstepping controller, sliding mode controller, and an adaptive neutral network control algorithm could not achieve optimal control performance with various constraints that include the inherent constraint of robot itself, such as the saturation and dead zone of thrusters, as well as some other constraints that are caused by the actual tasks, such as the state value threshold of the robot. In contrast, MPC is known as one of the few control methods that can explicitly handle system constraints systematically and can optimize control performance [18]. Usually, the MPC contains linear MPC and nonlinear MPC. The nonlinear MPC has more accurate tracking effects, as well as a higher cost of computation than linear MPC.

A comparison of the above-discussed control methods is shown in

Table 1. Comparison of the different control methods.

Method	Advantages	Disadvantages
PID	Simple; independent of system models	Single input and single output
LQR	Multiple input and multiple output	Dependent on system model
Sliding mode control	Robust to uncertain models and time-varying parameters	Chattering
Model predictive control	Deals with constraints; Multiple input and multiple output	Dependent on system model
Backstepping	Independent of accurate models; Suitable for nonlinear systems	As the complexity of the control problem increases, more parameters need to be adjusted
Fuzzy control	Independent of models	Over-dependence on expert experience
Neural network control	Adaptability, learning ability, and robustness	Dependent on a large number of training examples and needs a long training time

.

This article aims at proposing a formation tracking control that could be carried out and deployed easily on small spherical robots. The small spherical robot is born with some complex problems such as limited energy, computational ability, and perception ability; an inaccurate dynamic model; various constraints; etc. In order to deploy the formation algorithm to small spherical robots, the above issues must be considered. In addition, another challenge of formation control for small spherical robot lies in the avoidance of inner collisions. As such, some methods must be designed to overcome the above problems. The main contributions of this article are as follows:

• The multiple small spherical robot formation problem is split into trajectory tracking problems. The virtual structure is designed for obtaining the reference trajectory of each the robots in a formation. A model predictive-based formation tracking controller is proposed, and the ESO is designed to deal with the inaccurate dynamic model and other disturbances.
• To achieve the inner collision avoidance, a collision cost term is designed. The control input for each small spherical robot is determined by solving the local linear MPC optimization problem based on the state information of itself and the neighbors.
• A multi-small spherical robot formation simulation system is built based on a gazebo, and it is used for physical simulation to verify the effectiveness of the proposed algorithm.

The remaining parts of this article are organized as follows. The set up and modeling of the small spherical robot are described in Section 1. Next, the control approach was designed, and is detailed in Section 2. Then, the simulation analysis is described, including numerical and physical simulations, in Section 3, and this is followed by the experimental verification in Section 4. In Section 5, the simulation and experiment results are discussed. Finally, the conclusion are summarized in Section 6.

Notations: Bold symbols represent vectors and diag(·) denotes a diagonal operation.

2. Small Spherical Robot Set up and Modeling

2.1. The Mechanism of a Small Spherical Robot

A small spherical robot was designed in our laboratory based on the flexible movement characteristics of sea turtles in water, as shown in Figure 1. The robot was composed of two hemispheres that were joined with a mid-plate [19]. The upper hemisphere was divided into two parts, namely the water storage compartment and the sealed compartment. Water could enter the storage compartment through the four holes distributed around the water storage cabin when the small spherical robot dives into water, which helps with reducing the buoyancy of the robot. A sealed cabin was utilized to load the electrical system. The lower hemisphere was the vector propulsion system, which was bounded by two separate quarter spherical shells. The lower hemisphere could be opened or closed by servo motors on both sides [20]. A stereo camera, an inertial measurement unit (IMU), 12 pressure sensors, and an acoustic communication module were attached to the small spherical robot. For convenience, we designed a detachable battery pack that was fixed to the middle partition of the robot. The battery pack included three 7000 mAh lithium batteries, which were sealed in a cabin. The prototype of a small spherical robot is shown in Figure 2. In addition,

Table 2. The technical specification of the robot.

Items	Parameters
Dimension (Width × Length × Height)	30 cm × 60 cm × 30 cm
Mass in air	6.5 kg
Processors	NVIDIA Jetson Tk1 STM32F407
Max thrust	3.8 N
Motor	A2212
Sensors	Pressure sensor(MS5803-14BA ) IMU(3DM-GX5-45) Stereo camera Acoustic communication module(Micron Sonar)
Power	7.4 V rechargeable Ni-MH batteries (13,200 mAh)
Operation time	Average 100 min

lists the technical parameters of the robot.

An advantage of the small spherical robot lies in its vector propulsion mechanism, as shown in Figure 3. It can be seen that four sets of propulsion units comprised the propulsion system [19]. Each unit consisted of a hip, knee, and ankle joint, which could rotate around a self axis. As a result, the vector propulsion was very flexible. In order to ensure that the center of gravity and the centroid coincided, an “H”-shaped propulsion strategy was adopted [21]. The “H” shape propulsion strategy is shown in Figure 4; the strategy ensures that the hip and knee joint are kept still and only the ankle joint can be adjusted. In this situation, the driving force $\mathbf{\tau }\in {\mathbf{R}}^{3\times 1}$ is composed of three forces / moments: surge force ${\tau }_u$, heave force ${\tau }_w$, and yaw moment ${\tau }_r$.

2.2. Modeling of The Small Spherical Robot

Aiming at building the dynamic and kinematic models of the small spherical robot, a body reference frame ($O-X_B{Y}_B{Z}_B$) and an inertial reference frame ($O-X_I{Y}_I{Z}_I$) were established, as shown in Figure 5. The position and attitude angle of the robot, denoted as vector $\mathit{\eta }$, is described in the inertial coordinate system:

$$\mathit{\eta }=[x,y,z,\alpha ,\beta ,\varphi ],$$

(1)

where $x,y,z$ represent the position of the robot. $\alpha ,\beta ,\varphi$ are the Euler angle of the roll, pitch, and yaw. The motion states of the robot, denoted as vector $\mathbf{v}$, is represented in the body frame:

$$\mathbf{v}=[u,\nu ,w,p,q,r],$$

(2)

where, $u,\nu ,w$ are linear velocities in the x-direction, y-direction, and z-direction, respectively, which are denoted as the surge, sway, and heave, respectively [21]. $p,q,r$ are the angular velocities around the x-axis, y-axis, z-axis, respectively. As the “H”-shaped propulsion strategy was adopted for the trajectory tracking, the driving force could only be provided in the degrees of surge, yaw, and heave. Therefore, the degrees of sway, roll, and pitch will be neglected in this paper. Let $\mathit{\eta }=[x,y,z,\varphi ]$, $\mathbf{v}=[u,w,r]$. The kinematic model of the robot are shown as Equation (3) [20].

$$\dot{\mathit{\eta }}=\mathrm{J}\left(\mathit{\varphi }\right)\mathbf{v},$$

(3)

where

$$\mathrm{J}\left(\mathit{\varphi }\right)=\left[\begin{array}{ccc}cos\varphi & 0& 0\\ sin\varphi & 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right].$$

(4)

According to the Newton method, the dynamic model of the small spherical robot was established as follows [20]:

$$\mathit{M}\dot{\mathbf{v}}+\mathit{C}\left(\mathbf{v}\right)\mathbf{v}+\mathit{D}\left(\mathbf{v}\right)\mathbf{v}+\mathit{g}\left(\mathit{\eta }\right)=\mathbf{\tau },$$

(5)

where $\mathit{M}\in {\mathbf{R}}^{3\times 3}$ is the inertia and added mass matrix. $\mathit{C}\left(\mathbf{v}\right)\in {\mathbf{R}}^{3\times 3}$ is the Coriolis and centripetal matrix. $\mathit{D}\left(\mathbf{v}\right)\in {\mathbf{R}}^{3\times 3}$ is the damping matrix, which is positive-definite. $\mathit{g}\left(\mathit{\eta }\right)$ is the restoring force and torque vector. $\mathbf{\tau }$ is the driving force vector. As the structure of the robot was symmetrical, the inertial matrix and added matrix can be expressed by Equation (7) [20]. The Coriolis and centripetal matrix $\mathit{C}\left(\mathbf{v}\right)$ could be ignored because the speed of the robot was less than 0.5 m/s. Moreover, because the center of gravity and the buoyant center were almost at the same position, the restoring force and torque vector $\mathit{g}\left(\mathit{\eta }\right)$ could be set to 0 vector ($\mathit{g}\left(\mathit{\eta }\right)={\mathbf{0}}_{4\times 1}$). The simplified model is shown as follows:

$$\mathit{M}\dot{\mathbf{v}}+\mathit{D}\left(\mathbf{v}\right)\mathbf{v}=\mathbf{\tau },$$

(6)

where

$$\mathit{M}=\left[\begin{array}{ccc}{m}_{11}& 0& 0\\ 0& m_{22}& 0\\ 0& 0& m_{33}\end{array}\right]$$

(7)

$$\mathit{D}\left(\mathbf{v}\right)=\left[\begin{array}{ccc}{d}_{11}& 0& 0\\ 0& d_{22}& 0\\ 0& 0& d_{33}\end{array}\right]+\left[\begin{array}{ccc}{X}_u\left|u\right|& 0& 0\\ 0& Z_w\left|w\right|& 0\\ 0& 0& N_r\left|r\right|\end{array}\right]$$

(8)

$$\mathbf{\tau }={[{\tau }_u,{\tau }_w,{\tau }_r]}^T.$$

(9)

Equations (3) and (5) can be written as follows:

$$\dot{\mathit{\eta }}=\mathrm{J}\left(\mathit{\varphi }\right)\mathbf{v}$$

(10)

$$\dot{\mathbf{v}}={\mathit{M}}^{-1}\mathbf{\tau }-{\mathit{M}}^{-1}\mathit{D}\left(\mathbf{v}\right)\mathbf{v}.$$

(11)

3. Control Approach

In order to achieve formation control of multiple small spherical robots, a trajectory tracking-based formation strategy was proposed, which splits the formation problem into the trajectory tracking problem of a single robot, as shown in Figure 6. Firstly, the cubic spine method was utilized to obtain the desired formation trajectory based on the discrete formation path point. Then, according to the desired formation shape, the desired trajectory of each robot could be acquired by the virtual structure method. Finally, a tracking controller based on linear MPC and ESO was proposed, as shown in Figure 7. Detailed information of the formation tracking control are shown as follows.

3.1. Formation Problem Formulation

The trajectory tracking-based formation problem of multiple small spherical robots could be described in the following three mathematics problems.

• The tracking problem: ${\mathit{lim}}_{t\to \infty }\{{\mathit{q}}_i\left(t\right)-{\mathit{q}}_i^r\left(t\right)\}=0$.
• The formation problem: ${\mathit{lim}}_{t\to \infty }\{{\mathit{p}}_i\left(t\right)-{\mathit{p}}_j\left(t\right)\}={\mathit{d}}_{ij}$.
• The collision avoidance problem: $\Vert {\mathit{p}}_i\left(t\right)-{\mathit{p}}_j\left(t\right)\Vert \ge 2R$.

In the above problems, ${\mathit{p}}_i$ is the position of the small spherical robot i in the inertial frame. ${\mathit{q}}_i$ and ${\mathit{q}}_i^r$ represent the current- and desired-state information of the small spherical robot i. ${\mathit{d}}_{ij}$ is the relative distance between the robot i and robot j in an inertial coordinate system. R is the safety radius of the robot.

3.2. A Method for Generating the Reference Trajectory Based on Cubic Spline and Virtual Structure

A reference path of the multi-robot formation system is usually provided by the planning layer in the form of discrete points, and it generally only includes position information. However, a tracking controller based on MPC needs the state information, including the yaw, position, and speed. As such, the other pieces of reference state information need to be firstly obtained based on the cubic spline. Then, the reference state information of each robot in the formation can be acquired based on the virtual structure method.

3.2.1. Calculating the Formation Reference Information Based on Cubic Splines

For convenience of description, this section takes a two-dimensional format as an example to introduce the formation. Discrete formation path points provided by the path planning layer are denoted as ${\mathit{p}}_{dis}=[{\mathit{p}}_1,{\mathit{p}}_2,\cdots ,{\mathit{p}}_N]$. ${\mathit{p}}_i$ is the position of the i-th discrete path point. N is the number of the discrete path points. An interpolation method based on cubic splines was adopted to compute the other information of the robot. The cubic spline refers to the curve between each cell being a cubic equation, and the cubic spline equation must meet the following conditions.

• In the section between every two adjacent points $[{\mathit{p}}_i,{\mathit{p}}_{i+1}]$, $S\left(\mathit{p}\right)=S_i\left(\mathit{p}\right)$ is a cubic equation.
• Satisfy the interpolation condition that is $S\left({\mathit{p}}_i\right)=z_i$.
• The curve is smooth, which means that $S\left(\mathit{p}\right),\dot{S}\left(\mathit{p}\right),\ddot{S}\left(\mathit{p}\right)$ is continuous.

The function of the interpolated curve is denoted as $f(\dot{)}$, which is a piecewise function represented by multiple cubic equations. The interpolated curve is resampled at equal intervals, and the distance of the k-th resampling point is $d\left(k\right)=kT\times u_d$. u represents the surge speed. T is the sampling time. The cubic equation corresponding to the interval segment where the k-th sampling point is located is represented by a parameter equation as Equation (12).

$$\left\{\begin{array}{cc}\hfill x& =f_x\left(s\right)\hfill \\ \hfill y& =f_y\left(s\right)\hfill \end{array},\right.$$

(12)

where s is the distance. The position, attitude angle, and velocity information of the k-th sampling point are represented in Equation (13).

$$\left\{\begin{array}{c}{x}_r=f_x\left(d\left(k\right)\right)\hfill \\ y_r=f_y\left(d\left(k\right)\right)\hfill \\ {\varphi }_r=arctan\left({\displaystyle \frac{{\dot{f}}_y}{{\dot{f}}_x}}\right)\hfill \\ u_r=u_d\hfill \\ r_r={\displaystyle \frac{({\ddot{f}}_y{\dot{f}}_x-{\ddot{f}}_x{\dot{f}}_y)u_d}{{\left({{\dot{f}}_x}^2+{\dot{f}}_y^2\right)}^{1.5}}}\hfill \end{array}\right.$$

(13)

The above obtains the state information of the k-th reference point in the two-dimensional plane. The state information of the heave degree of freedom can be expressed as $z_r={\omega }_{d}T, {\omega }_r={\omega }_d$. The state vector of the formation reference point based on cubic spline resampling is denoted as ${\mathit{q}}^{\mathit{r}}={[x_r,y_r,z_r,{\varphi }_r,u_r,{\omega }_r,r_r]}^T$.

3.2.2. Modeling of the Virtual Structure in Three Dimensions

The global reference information could be obtained according to the above section. In order to achieve formation tracking control, a reference state information of each robot from the formation system needs to be acquired. As such, this section will describe how to obtain this information based on the virtual structure method. The virtual structure method treats the formation system as a rigid body. If a rigid body moves in space and each point on it has a trajectory corresponding to the motion of the rigid body, then each node of the virtual structure generates a motion trajectory. In this paper, each small spherical robot is seen as a node of the virtual structure, and the center of the virtual structure is seen as a virtual node. It is set that the virtual node moves along the formation path provided by the planning layer. A schematic diagram of the reference trajectory generation based on a virtual structure is depicted in Figure 8. The green plane in the figure is a triangular virtual structural plane. For the convenience of description, a virtual structure coordinate was established and denoted as $O_S{x}_S{y}_S\left\{S\right\}$. $\left\{S\right\}$ locates the origin as the center of the triangle virtual structure. The virtual structure coordinate moves along with the virtual node. The virtual structure coordinate system takes the tangent of the desired formation trajectory as the $y_S$. The pink plane ($plan{e}_S$) in the figure is perpendicular to the $y_S$ axis. The projection of the $z_E$ axis of the global coordinate system on the $plan{e}_S$ is set as $z_S$. $x_S$ could be determined according to the right hand rule, where ${\psi }^r,{\theta }^r$ are the pitch angle and heading angle of the virtual coordinate system relative to the inertial coordinate system, respectively.

The position coordinates of the virtual node (or formation reference point) in the inertial coordinate system is marked as $\mathit{p}\in {\mathbf{R}}^3$. The position coordinate of the i-th node is labeled as ${\mathit{p}}_i^S\in {\mathbf{R}}^3$ in the virtual structure coordinate, and it is labeled as ${\mathit{p}}_i\in {\mathbf{R}}^3$ in the inertial frame. ${\mathit{p}}_i\in {\mathbf{R}}^3$ could be presented as Equation (14):

$${\mathit{p}}_i=\mathit{p}+R({\psi }^r,{\theta }^r)L{\mathit{p}}_i^s,$$

(14)

where L is a formation-scaling coefficient matrix and is represented as the following:

$$L=\left[\begin{array}{c}\hfill \begin{array}{ccc}l& 0& 0\\ 0& l& 0\\ 0& 0& l\end{array}\end{array}\right],$$

(15)

where $R({\psi }^r,{\theta }^r)$ is the transformation matrix from the virtual structure coordinate to the inertial coordinate, and it can be represented as Equation (16).

$$R({\psi }^r,{\theta }^r)=\left[\begin{array}{c}\hfill \begin{array}{ccc}cos{\theta }^{r}cos{\psi }^r& cos{\theta }^{r}sin{\psi }^r& sin{\theta }^{r}sin{\psi }^r\\ -cos{\theta }^{r}sin{\psi }^r& cos{\theta }^{r}cos{\psi }^r& -sin{\theta }^{r}cos{\psi }^r\\ -sin{\theta }^r& sin{\theta }^r& cos{\theta }^r\end{array}\end{array}\right],$$

(16)

where ${\mathit{p}}_i^s,i=(1,2,\cdots ,N)$ could be obtained based on the formation set in the virtual structure coordinate. ${\mathit{p}}_i$ represents the desired position of the i robot, which could be obtained based on Equation (14). The other state information of state vector ${\mathit{q}}_i^r$ is consistent with the state vector ${\mathit{q}}^r$.

3.3. Formation Tracking Controller Based on ESO-MPC

3.3.1. Linear Predictive Model of the Small Spherical Robot

Because the lower control input is the PWM (pulse width modulation) signal driving the thruster to generate the thrust, in the predictive model, the relationship between the robot states (the position x, y, and the yaw angle $\varphi$) and the driving force is considered. The dynamics of the robot reflect the relationship between the velocity and the driving force, and the kinematics describes the relationship between the robot states and the velocity. Therefore, the predictive model for the trajectory tracking is established by combining Equations (10) and (11) as follows:

$$\begin{array}{cc}\hfill \dot{\mathbf{x}}& =\left[\begin{array}{c}\mathrm{J}\left(\mathit{\varphi }\right)\mathbf{v}\\ {\mathrm{M}}^{-\mathbf{1}}(\mathbf{\tau }-\mathit{D}\left(\mathbf{v}\right)\mathbf{v})\end{array}\right]\hfill \\ \hfill & =\mathbf{f}(\mathbf{x},\mathbf{\tau })\hfill \end{array}.$$

(17)

Considering the limited computation ability of the small spherical robot, the linear MPC was designed. As such, it is essential to linearize Equation (17) in order to develop a state-space-based model predictive control algorithm. When referring to the system represented by Equation (17), $({\mathbf{x}}_0,{\mathbf{\tau }}_0)$ is a working point. It is assumed that a sustained force ${\mathbf{\tau }}_0$ is applied to the system in the future, and then the state trajectory that can be presented as Equation (18) can be obtained. At the same time, a series of reference points from the above state trajectory can be obtained and are marked as $({\mathbf{\tau }}_0,{\mathbf{x}}_r\left(1\right)),({\mathbf{\tau }}_2,{\mathbf{x}}_r\left(2\right))$⋯$({\mathbf{\tau }}_k,{\mathbf{x}}_r\left(k\right))$.

$$\begin{array}{cc}\hfill {\dot{\mathbf{x}}}_r\left(t\right)& =\mathbf{f}({\mathbf{x}}_r\left(t\right),\mathbf{\tau }\left(t\right))\hfill \\ \hfill \mathbf{\tau }\left(t\right)& ={\mathbf{\tau }}_0\hfill \\ \hfill {\mathbf{x}}_r\left(0\right)& ={\mathbf{x}}_0\hfill \end{array}$$

(18)

Due to the current state of the robot being closest to the first reference point, the Taylor series expansion on the closest reference point was applied to Equation (17), and the high-order terms were ignored in order to obtain the following expression:

$$\begin{array}{cc}\hfill \dot{\mathbf{x}}\left(t\right)& =A_t(\mathbf{x}\left(t\right)-{\mathbf{x}}_r\left(0\right))+B_t(\mathbf{\tau }\left(t\right)-{\mathbf{\tau }}_0)+f({\mathbf{x}}_r\left(0\right),{\mathbf{\tau }}_0)\hfill \\ \hfill & =A_t\mathbf{x}\left(t\right)+B_t\mathbf{\tau }\left(t\right)+f({\mathbf{x}}_r\left(0\right),{\mathbf{\tau }}_0)-A_t{\mathbf{x}}_r\left(0\right)-B_t{\mathbf{\tau }}_0\hfill \end{array},$$

(19)

where $d_t=f({\mathbf{x}}_r\left(0\right),{\mathbf{\tau }}_0)-A_t{\mathbf{x}}_r\left(0\right)-B_t{\mathbf{\tau }}_0$ represents the error between the next state, which is calculated from the linearization equation and the next state that is obtained from the nonlinear equation. By using the approximately discrete method, the discrete form of Equation (19) can be expressed as follows:

$$\mathbf{x}(k+1)=\tilde{\mathbf{A}}\mathbf{x}\left(k\right)+\tilde{\mathbf{B}}\mathbf{\tau }\left(k\right)+d\left(k\right),$$

(20)

where $\tilde{\mathbf{A}}=I+T{A}_t$, $\tilde{\mathbf{B}}=T{B}_t$, $d\left(k\right)=f({\mathbf{x}}_r\left(0\right),{\mathbf{\tau }}_0)-\tilde{\mathbf{A}}{\mathbf{x}}_r\left(0\right)-\tilde{\mathbf{B}}{\mathbf{\tau }}_0$, I is an identity matrix, and T is the sampling time. $\tilde{\mathbf{A}},\tilde{\mathbf{B}},d\left(k\right)$ update depending on the updating states of the robot.

If Equation (19) is set as the predictive model, the optimization objective is the control input vector $\mathbf{\tau }$, which is unable to avoid a sudden increment of the control input and will affect the continuity of the control input. Therefore, Equation (19) is transformed as follows:

$$\mathbf{\xi }\left(k\right)=\left[\begin{array}{c}\tilde{\mathbf{x}}\left(k\right)\\ \tilde{\mathbf{\tau }}(k-1)\end{array}\right].$$

(21)

A new state-space expression is obtained as follows:

$$\begin{array}{cc}\hfill \mathbf{\xi }(k+1)& =\mathbf{A}\mathbf{\xi }\left(k\right)+\mathbf{B}\Delta \mathbf{\tau }\left(k\right)+d_e\left(k\right)\hfill \\ \hfill \mathit{\eta }\left(k\right)& =\mathit{C}\mathbf{\xi }\left(k\right),\hfill \end{array}$$

(22)

where $d_e\left(k\right)={[d\left(k\right),0_3]}^T$, $0_3$ is a three-dimensional all-zero column vector. The concrete description of $\tilde{\mathbf{A}}$ and $\tilde{\mathbf{B}}$ can be found in reference [20].

$$\mathbf{A}=\left[\begin{array}{cc}\tilde{\mathbf{A}}& \tilde{\mathbf{B}}\\ {\mathbf{0}}_{m\times n}& {\mathbf{I}}_m\end{array}\right]$$

(23)

$$\mathbf{B}=\left[\begin{array}{c}\tilde{\mathbf{B}}\\ {\mathbf{I}}_m\end{array}\right]$$

(24)

$$\mathit{C}=\left[\begin{array}{cc}{\mathbf{I}}_n& {\mathbf{0}}_{n\times m}\end{array}\right].$$

(25)

3.3.2. Design of the ESO Based on the State Space

Although Equation (20) shows the predictive model, the realization of the tracking control produces some problems. First, it is difficult to obtain accurate dynamic model parameters such as $\mathit{M}$ and $\mathit{D}\left(\mathbf{v}\right)$. Second, there are unknown disturbances in underwater environments. Third, the small spherical robot lacks a sensor to measure the velocity state. In response to the above issues, this section designs an extended state observer, which sees the nonlinear terms in the dynamic model and environmental disturbance as the lumped disturbances. The observed value of the lumped disturbances will be compensated to control inputs. The observed velocity values are used as the speed of the robot.

The inaccurate hydrodynamic coefficients $\mathit{M}$ and $\mathit{D}\left(\mathbf{v}\right)$ in the dynamic model in Equation (6) can be divided into two parts, i.e., the nominal value part and the uncertain value part, which denote the error between the true value and the nominal value [22], respectively. Thereinto, the nominal value part can be obtained by model identification [20]. However, the nominal value of the body mass matrix $\mathit{M}$ is more reliable. Therefore, it will simplify the model expression by modeling only with the explicit expression of $\mathit{M}$. With the explicit expression of M and the implicit expression of the hydrodynamics coefficient $\mathit{D}\left(\mathbf{v}\right)$, the dynamic model of the spherical robot is described as follows [22]:

$$\mathit{M}\dot{\mathbf{v}}=\mathcal{U}\left(\mathbf{v}\right)+{\mathbf{\tau }}_E+\mathbf{\tau },$$

(26)

where $\mathcal{U}\left(\mathbf{v}\right)=\mathit{D}\left(\mathbf{v}\right)\mathbf{v}$ and ${\mathbf{\tau }}_E$ are the environmental disturbance.

There are two primary assumptions for the dynamics model (26), which are considered in the following ESO design.

Assumption 1

(see in [23]). Due to the velocity-related property of the hydrodynamics item $\mathcal{U}\left(\mathit{v}\right)$, the sum of the partial derivatives is assumed to be bounded.

$$\parallel \mathcal{U}(\mathit{v},t)\parallel +\parallel \partial \mathcal{U}(\mathit{v},t)/\partial t\parallel +\parallel \partial \mathcal{U}(\mathit{v},t)/\partial \mathit{v}\parallel \le c_1.$$

(27)

Assumption 2

(see in [23]). Considering the energy limitations of actuators and external forces, the following assumption is reasonable. Namely,

$$∥{\mathbf{\tau }}_E∥+∥{\dot{\mathbf{\tau }}}_E∥+\parallel \mathbf{\tau }\parallel +\parallel \dot{\mathbf{\tau }}\parallel \le c_2.$$

(28)

The uncertain parts of the dynamic model and environmental unknown disturbances are defined as a lumped disturbance vector denoted as $\Gamma$. Namely,

$$\Gamma ={\mathit{M}}^{-1}(\mathcal{U}\left(\mathbf{v}\right)+{\mathbf{\tau }}_E)+({\mathit{M}}^{-1}-{\left({\mathit{M}}^{*}\right)}^{-1})\mathbf{\tau },$$

(29)

where $\mathit{M},{\mathit{M}}^{*}$ represent the true value and the nominal value, respectively. Then, Equation (26) can be written as per the following:

$$\dot{\mathbf{v}}=\Gamma +{\left({\mathit{M}}^{*}\right)}^{-1}\mathbf{\tau }.$$

(30)

Let $\Gamma$ be the expanded state, and combine Equations (10) and (30), then the ESO is designated as the following:

$$\left\{\begin{array}{cc}\hfill \dot{\widehat{\mathit{\eta }}}& =\mathrm{J}\left(\varphi \right)\widehat{\mathbf{v}}+{\mathbf{\beta }}_1(\mathit{\eta }-\widehat{\mathit{\eta }})\hfill \\ \hfill \dot{\widehat{\mathbf{v}}}& =\widehat{\Gamma }+{\left({\mathit{M}}^{*}\right)}^{-1}\mathbf{\tau }+{\mathbf{\beta }}_2\mathrm{J}{\left(\varphi \right)}^T(\mathit{\eta }-\widehat{\mathit{\eta }})\hfill \\ \hfill \dot{\widehat{\Gamma }}& ={\mathbf{\beta }}_3\mathrm{J}{\left(\varphi \right)}^T(\mathit{\eta }-\widehat{\mathit{\eta }})\hfill \end{array}\right.,$$

(31)

where $\widehat{\mathit{\eta }},\widehat{\mathbf{v}},\widehat{\Gamma }$ are the estimated vectors of $\mathit{\eta },\mathbf{v},\Gamma$. In addition, ${\mathbf{\beta }}_1,{\mathbf{\beta }}_2,{\mathbf{\beta }}_3$ are positive constant matrices.

$$\left\{\begin{array}{c}{\mathbf{\beta }}_1=\mathrm{diag}({\beta }_{11},{\beta }_{12},{\beta }_{13},{\beta }_{14})\hfill \\ {\mathbf{\beta }}_2=\mathrm{diag}({\beta }_{21},{\beta }_{22},{\beta }_{23},{\beta }_{24})\hfill \\ {\mathbf{\beta }}_3=\mathrm{diag}({\beta }_{31},{\beta }_{32},{\beta }_{33},{\beta }_{34})\hfill \end{array}\right..$$

(32)

When the inaccurate dynamic model and environmental disturbance are considered, the predictive model of the small spherical robot will be written as per the following (according to Equation (22)).

$$\begin{array}{cc}\hfill \mathbf{\xi }(k+1)& =\mathbf{A}\mathbf{\xi }\left(k\right)+\mathbf{B}\Delta \mathbf{\tau }\left(k\right)+d_e\left(k\right)+\mathit{D}\Gamma \left(k\right)\hfill \\ \hfill \mathit{\eta }\left(k\right)& =\mathit{C}\mathbf{\xi }\left(k\right).\hfill \end{array}$$

(33)

3.3.3. Cost Function Design of the MPC

When aiming at achieving formation control, the cost function of the i-th small spherical robot at the sampling time k is designed as the following based on the formation problem formulation described in Section 2.1:

$${\mathit{J}}_{total}^i\left(k\right)={\mathit{J}}_{fom}^i\left(k\right)+{\mathit{J}}_{in}^i\left(k\right).$$

(34)

The above cost function consists of two parts. ${\mathit{J}}_{fom}^i\left(k\right)$ is designed as being responsible for the tracking trajectory and keeping the relative distance with the other robots at the same time. ${\mathit{J}}_{in}^i\left(k\right)$ is designed for preventing the collisions among the small spherical robots. ${\mathit{J}}_{fom}^i\left(k\right)$ is defined as follows:

$$\begin{array}{cc}\hfill {\mathit{J}}_{fom}^i\left(k\right)=& \sum _{s=1}^{N_p}{∥{\mathit{\eta }}^i(k+s|k)-{\mathit{\eta }}_{ref}^i(k+s|k)∥}_{Q_i}^2\hfill \\ \hfill +& \sum _{s=1}^{N_p}{∥{\mathit{p}}^i(k+s|k)-{\mathit{p}}^j(k+s|k)-({\mathit{p}}_{ref}^i(k+s|k)-{\mathit{p}}_{ref}^j(k+s|k))∥}_{Q_{ij}}^2\hfill \\ \hfill +& \sum _{j=1}^{N_c}{∥\Delta \mathbf{\tau }(k+s|k)∥}_{R_i}^2+\rho {\epsilon }^2\hfill \end{array},$$

(35)

where $N_p$ is the predictive horizon, $N_c$ is the controlling horizon, $\rho$ is the weighting factor, and $\epsilon$ is the relaxation factor. $Q_i,R_i,Q_{ij}$ are the weighting matrices. The first term of Equation (35) is to ensure that the state vector $\mathit{\eta }$ of the robot approaches the desired state vector. The second term guarantees the distance between the i-th robot and the j-th robot. The third term aims at minimizing the increment of control input. The purpose of the last term is to prevent an optimization problem from being unsolved.

When referring to the collision avoidance problem for the spherical robots, a concept of priority was proposed. Each robot in the formation is marked a label that represents priority levels. The label updates with the state changes of the robot. If a collision occurs, the robot at the lower level needs to adjust the state. The priority from high to low is set as per the following:

• Robots with urgent tasks such as target hunting.
• Robots that have minor tracking errors.
• Robots that could obtain the information of the other robots.

The above rules can be implemented according to specific circumstances. The ${\mathit{J}}_{in}^i\left(k\right)$ is written as follows:

$$J_{in}^i\left(k\right)=\left\{\begin{array}{c}0,if{d}_{ij}\left(k\right)>2R\hfill \\ 0,if{d}_{ij}\left(k\right)\le 2R,le{v}_i\left(k\right)>le{v}_j\left(k\right)\hfill \\ \sum _{s=1}^{N_p}-S_{in}(d_{ij}(k+s|k)-2R),\hspace{1em}if{d}_{ij}\left(k\right)\le 2R,le{v}_i\left(k\right)\le le{v}_j\left(k\right)\hfill \end{array}\right.,$$

(36)

where

$$\begin{array}{c}{d}_{ij}\left(k\right)=\sqrt{{(x_i\left(k\right)-x_j\left(k\right))}^2+{(y_i\left(k\right)-y_j\left(k\right))}^2+{(z_i\left(k\right)-z_j\left(k\right))}^2}\hfill \\ d_{ij}(k+s|k)=({(x_i(k+s|k)-x_j(k+s|k))}^2+{(y_i(k+s|k)-y_j(k+s|k))}^2\hfill \\ +{(z_i(k+s|k)-z_j(k+s|k))}^2{)}^{0.5}\hfill \end{array},$$

(37)

where $S_{in}$ represents the weight as a positive constant value. $le{v}_i\left(k\right),le{v}_j\left(k\right)$ represent the levels of the i-th robot and j-th robot.

3.3.4. Design of the Optimization Problem Based on ESO-MPC

In order to achieve multi-small spherical robot formation tracking, the following optimization formula of MPC based on ESO was established. At each sampling time k, the first term of the vector $\Delta \mathit{U}\left(k\right)$, which is the solution to the optimization problem (38) and the observed value of the lumped disturbance $\widehat{\Gamma }$, will be the real control input increment at the next sampling time.

$$\begin{array}{cc}\hfill \Delta \mathbf{U}\left(k\right)& =arg\underset{\Delta \mathit{U}}{min}{\mathit{J}}_{total}^i\left(k\right)\hfill \\ \hfill s.t.& d_{ij}\left(k\right)=\sqrt{{(x_i\left(k\right)-x_j\left(k\right))}^2+{(y_i\left(k\right)-y_j\left(k\right))}^2+{(z_i\left(k\right)-z_j\left(k\right))}^2}\hfill \\ \hfill & \mathbf{\xi }(k+1)={\mathbf{A}}^{{}^{\prime }}\mathbf{\xi }\left(k\right)+{\mathbf{B}}^{{}^{\prime }}\Delta \mathbf{\tau }\left(k\right)+d_e\left(k\right)+\mathit{D}\Gamma \left(k\right)\hfill \\ \hfill & \mathit{\eta }(k+1)=\mathit{C}\mathbf{\xi }(k+1)\hfill \\ \hfill & \mathbf{Y}\left(k\right)=[\mathit{\eta }(k+1),\cdots ,\mathit{\eta }(K+N_p)]\hfill \\ \hfill & \mathbf{\xi }\left(k\right)={[\mathit{\eta }\left(k\right),\widehat{\mathbf{v}}\left(k\right),\mathbf{\tau }(k-1)]}^T\hfill \\ \hfill & \dot{\widehat{\mathit{\eta }}}=\mathrm{J}\left(\varphi \right)\widehat{\mathbf{v}}+{\mathbf{\beta }}_1(\mathit{\eta }-\widehat{\mathit{\eta }})\hfill \\ \hfill & \dot{\widehat{\mathbf{v}}}=\widehat{\Gamma }+{\left({\mathit{M}}^{*}\right)}^{-1}\mathbf{\tau }+{\mathbf{\beta }}_2\mathrm{J}{\left(\varphi \right)}^T(\mathit{\eta }-\widehat{\mathit{\eta }})\hfill \\ \hfill & \dot{\widehat{\Gamma }}={\mathit{\beta }}_3\mathrm{J}{\left(\varphi \right)}^T(\mathit{\eta }-\widehat{\mathit{\eta }})\hfill \\ \hfill & \Delta {\mathbf{\tau }}_{min}\le \Delta \mathbf{\tau }(k+i-1)\le \Delta {\mathbf{\tau }}_{max}\hfill \\ \hfill & {\mathbf{\tau }}_{min}\le \mathbf{\tau }(k+i)\le {\mathbf{\tau }}_{max}\hfill \\ \hfill & \Delta \mathbf{\tau }(k+i-1)=\mathbf{\tau }(k+i-1)-\mathbf{\tau }(k+i-2)\hfill \\ \hfill & \Delta \mathit{U}={[\Delta \mathbf{\tau }\left(k\right),\cdots ,\Delta \mathbf{\tau }(k+N_p-1)]}^T\hfill \\ \hfill & i=[1,2,\cdots ,N_p]\hfill \end{array},$$

(38)

where $\Delta {\mathbf{\tau }}_{min}$ and $\Delta {\mathbf{\tau }}_{max}$ are the lower and upper limit of the control increment, respectively. ${\mathbf{\tau }}_{min}$ and ${\mathbf{\tau }}_{max}$ are the lower and upper limit of the control input, respectively.

4. Simulation Results

In order to verify the feasibility of the proposed formation controller, which is briefly named VS-ESO-MPC below, serious simulation experiments—including numerical simulations and physical simulations for the multi-small spherical robot system—were conducted by using professional software.

4.1. Numerical Simulation Results

In this section, we set up two simulation scenarios. A formation tracking simulation was conducted in the first scenario without external disturbances. In the second scenario, we tested the disturbance rejection by introducing considerable disturbances. All of the simulations were conducted by using MATLAB 2015a on a Windows 7 equipped with an Inter core i7 CPU at 3.6 GHz and 8 GB of RAM.

4.1.1. Simulation Setup

1. The dynamic model parameters setup. In the simulation, the formation system consists of three small spherical robots with the same dynamic model parameters. The concrete value of the parameters were identified in reference [20].

2. Constraints setup. The maximum thrust of a single water jet thruster is 3 N. According to the “H”-shaped distribution of the four thrusters, the control input vector needs to satisfy the following equation:

$$\left[\begin{array}{c}-6\\ -3\\ -0.5\end{array}\right]\le \mathbf{\tau }\le \left[\begin{array}{c}6\\ 3\\ 0.5\end{array}\right].$$

(39)

The control input increment vector satisfies the following constraint:

$$\left[\begin{array}{c}-0.3\\ -0.3\\ -0.1\end{array}\right]\le \Delta \mathbf{\tau }\le \left[\begin{array}{c}0.3\\ 0.3\\ 0.1\end{array}\right].$$

(40)

3. Parameters of the VS-ESO-MPC setup. The sampling period is $T=0.05 \mathrm{s}$. The control horizon $N_c$ is almost half of the $N_p$. According to the experience and the simulation results, $N_p$ is set to 25 and $N_c$ is set to 15.

4. Formation reference path setup. In a multi-small spherical robot formation system, the formation path is calculated by the planning layer based on the actual environment. The planning layer sends a series of discrete points along the formation path to the spherical robot. In order to be closer to the real experiment, a series of discrete points on the $x-y$ plane is set. Then, the cubic spine is adopted to compute the reference state trajectory, which includes the position and yaw angle. The discrete formation reference point is set as $positio{n}_X={[-4,-2,0.0,2,4,6,10,12,15,18]}^T$ and $positio{n}_Y={[1.2,0.6,0.0,1.5,3.8,5.0,3.0,2.0,-2,1]}^T$. The formation trajectory is based on the above discrete points and is shown in Figure 9. Figure 9b shows the heading angle after equal-length resampling. The reference trajectory in the depth is set as $z_d\left(t\right)=0.5t$.

4.1.2. Formation Tracking without Disturbance

A triangular formation simulation was conducted in three-dimensional space. The weight matrix in Equation (35) is $R_1=R_2=R_3=0.01\times I_4$, $Q_1=\mathrm{diag}(1,0.5,1,1)$, $Q_2=\mathrm{diag}(1,2,1,1),Q_3=I_4,Q_{12}=Q_{23}=Q_{13}=0.15\times I_4$. The initial states of the three robots and the formation shape are as per the following:

$$\begin{array}{cc}\hfill d_{12}& =-d_{21}={[0,4,0]}^T,d_{13}=-d_{31}={[0,2,-2]}^T,d_{23}=-d_{32}={[0,-2,-2]}^T\hfill \\ \hfill {\mathit{q}}_{\mathbf{1}}& ={[0,1.5,0,0,0,0,0]}^T\hfill \\ \hfill {\mathit{q}}_{\mathbf{2}}& ={[-0.8,-1,1,0,0,0,0]}^T\hfill \\ \hfill {\mathit{q}}_{\mathbf{3}}& ={[0,1,1,0,0,0,0]}^T\hfill \end{array},$$

(41)

where $d_{ij}$ represents the three-dimension position vector difference between the i-th and the j-th positions. ${\mathit{q}}_{\mathbf{i}}$ is the initial state vector including $x,y,z,\varphi ,u,w,r$.

The simulation results are illustrated in Figure 10. It can be seen that the initial positions of the three robots exhibited a random triangular distribution, which gradually converged into the expected triangular formation. In addition, when the robots formed a triangular formation, Robot 1 and Robot 2 did not converge to their corresponding reference trajectories. However, while maintaining the formation, the three robots gradually converged to the reference trajectories and achieved a triangular formation in three-dimensional space. Figure 11 shows the error between the state variables of each robot and the reference state variables during the formation process, which is defined as the trajectory tracking error for convenience of description. As shown in the figure, the trajectory tracking error gradually converged to 0. The state difference between the two adjacent robots is presented in Figure 11b. The position difference between the adjacent robots gradually converged to the expected formation setting, and the other states difference converged to 0. In order to analyze the result more clearly,

Table 3. Root mean square error of the trajectory tracking and formation constraints.

Robot Label	$\mathbf{\Delta }\mathit{x}$ (m)	$\mathbf{\Delta }\mathit{y}$ (m)	$\mathbf{\Delta }\mathit{z}$ (m)	$\mathbf{\Delta }\mathit{\varphi }$ (rad)
1	0.135	0.087	0.020	0.043
2	0.120	0.024	0.010	0.009
3	0.130	0.081	0.010	0.041
1–2	0.057	0.100	0.005	0.041
1–3	0.029	0.098	0.004	0.023
2–3	0.036	0.059	0.001	0.003

lists the root mean square error of the trajectory tracking and the root mean square error of the formation. In the table, 1–2 represents the error between the actual distance and the expected distance between Robot 1 and Robot 2, which is referred to as the formation error for convenience of description. As can be seen, the trajectory tracking errors of the three robots in the x direction was relatively large, but the formation errors in the x direction was very small. The reason for this was that the formation controller had constraints on the formation shape, and there was a weight adjustment between the trajectory tracking and formation constraints. When the weight of the formation constraint term in the cost function is large, more emphasis will be placed on the formation constraint. The maximum error in the table was 0.1354 m, which was less than the diameter of the small spherical robot (0.3 m), which verified that three robots were able to finish the triangle formation tracking task based on the VS-ESO-MPC. The control inputs of the three robots during the formation process are shown in Figure 12. It can be seen that all control input quantities were within the constraint range.

In order to verify the advantages of the proposed method, a simulation based on the backstepping method was carried out. The formation shape and the initial positions of all the robots were consistent with that in the simulation based on the VS-MPC-ESO. The simulation results are illustrated in Figure 13. It can be seen that the three robots could form the desired shape. However, it was obvious that the formation was not able to track the desired trajectory accurately, and the real trajectory of each robot possessed slight oscillations. In addition, the control inputs of each of the robots based on backstepping are shown in Figure 14. As the backstepping could not deal with the constraints, the control input $\tau 1$ of Robot 2 exceeded the limit (the limit of $\tau 1$ is 6 N).

According to the comparison, the proposed formation method is obviously preferred over the backstepping method in terms of the aspects of dealing with constraints and formation trajectory tracking.

4.1.3. Formation Tracking with Disturbance

In order to verify the anti-disturbance ability of the ESO in interference environments, the following simulation experiment was designed: Add different frequency interference signals to the three robots. Then, $disturbanc{e}_1\left(t\right)=disturbanc{e}_3\left(t\right)=3sin(0.01\pi )t$ is added to Robot 1 and Robot 3. Next, $disturbanc{e}_2\left(t\right)=3sin(0.04\pi )t$ is added to Robot 2. The initial states of the three robots and the formation shape were set as per the following:

$$\begin{array}{cc}\hfill d_{12}& =d_{21}={[0,2,0]}^T,d_{13}=-d_{31}={[0,2,-2]}^T,d_{23}=d_{32}={[0,-2,-2]}^T\hfill \\ \hfill {\mathit{q}}_{\mathbf{1}}& ={[0,0,0,0,0,0,0]}^T\hfill \\ \hfill {\mathit{q}}_{\mathbf{2}}& ={[-0.5,-3,1,0,0,0,0]}^T\hfill \\ \hfill {\mathit{q}}_{\mathbf{3}}& ={[0,-2,1,0,0,0,0]}^T\hfill \end{array}.$$

(42)

The multi-small spherical robots formation tracking result is illustrated in Figure 15. As shown in the figure, the three robots respectively formed the expected formation and tracked the expected trajectory. The real trajectory of each of the robots possessed slight fluctuations, which was caused by added disturbances.

Table 4. Root mean square error of the trajectory tracking and the formation with disturbance.

Robot Number	$\mathbf{\Delta }\mathit{x}$ (m)	$\mathbf{\Delta }\mathit{y}$ (m)	$\mathbf{\Delta }\mathit{z}$ (m)	$\mathbf{\Delta }\mathit{\varphi }$ (rad)
1	0.134	0.112	0.010	0.073
2	0.118	0.071	0.023	0.134
3	0.172	0.107	0.011	0.092
1–2	0.181	0.112	0.006	0.177
1–3	0.055	0.063	0.001	0.080
2–3	0.212	0.115	0.006	0.205

lists the root mean square error of the trajectory tracking and the root mean square error of the formation. Compared with Table 3, the root mean square error of the yaw angle increased, which was due to the higher-frequency sinusoidal interference added to Robot 2. The maximum error in a three-dimensional formation with interference is 0.66BL (body length), which is less than the diameter of the robot and demonstrates the effectiveness of the formation control method with the ESO.

4.2. Physical Simulation Results

Physical simulations can take into account the real properties of robots, environmental properties, etc., and it can be more closely related to real experiments when compared with numerical simulations. In order to further verify the validity of the VS-ESO-MPC, physical simulations were conducted under two physical simulation scenarios, and these were based on the self-building multi-small spherical robot formation simulation platform.

4.2.1. Self-Building Multi-Small Spherical Robot Formation Simulation Platform

ROS/Gazebo is an open-source physical simulation platform that is widely used in robot development and multi robot coordination simulation. The Gazebo-based simulation platform mainly includes small spherical robot models, unmanned aerial vehicle models, and environmental plugins. Unmanned aerial vehicles are used for the localization of the spherical robots based on the global visual location method [24]. Figure 16 shows the graphical interface loaded in the multi-robot formation simulation.

The simulation platform adopted the ROS framework (shown in Figure 17) and was ran on the Ubuntu 14.04 system platform. The ROS-based framework mainly consists of a drone model package, a drone control package, a drone vision package, a small spherical robot model package, a spherical robot control package, and a spherical robot driving package. When the simulation platform runs, all of the models, including a drone, three small spherical robot models, a environment model, as well as control algorithms (including the control algorithms of three spherical robots, an unmanned aerial vehicle, and a vision-based positioning and path planning algorithm) needed to be loaded. Due to the heavy computation, the simulation platform was implemented using three computers, as shown in Figure 18. Each computer was in charge of the control algorithm. In addition, one of the three computers was simultaneously responsible for drone control, vision-based robot position calculation, and interface display work. The communication between the multiple computers adopted the communication software multimasters kill [25].

4.2.2. The Gazebo-Based Simulation Setup

To verify the effectiveness of the algorithm from multiple perspectives, two scenes were set. One was a simple water environment. The other was a complex water environment with islands, as shown in Figure 16. The khaki area represents the land area, while the dark green area represents the water area. Compared with the trajectory in a single water environment, the trajectory in a complex environment needs to avoid land areas, so the heading angle was constantly changing, which increases the difficulty for the robots in achieving more stable and accurate formations. In the simulation, two formation shapes were designed, as shown in Figure 19. Due to the issue of communication, the validation of the formation method for multi-spherical robots was carried out in a two-dimensional plane. The freedom of heave was completely decoupled from the surge and sway degrees of freedom. Therefore, the validation of the two-dimensional plane formation method could also prove the effectiveness of the formation method in the heave degree of freedom.

4.2.3. Triangle Formation in a Single Water Scene

The start point and final point in the single scene were set as $(x,y)=(-4\mathrm{m},-3.1\mathrm{m})$ and $(x,y)=(6.5\mathrm{m},7.3\mathrm{m})$, respectively. The formation shape was a triangle, as shown in Figure 19b. The initial position of the three spherical robots were $positio{n}_1=(-4.5\mathrm{m},-4.5\mathrm{m})$, $positio{n}_2=(-7.5\mathrm{m},-5.5\mathrm{m})$, and $positio{n}_3=(-5.3\mathrm{m},-7\mathrm{m})$. The real formation tracking result is depicted in Figure 20. It can be seen that the initial shape composed of three robots was different than the desired formation shape, and each robot was far away from the desired trajectory. Under the adjustment of the controller, the three robots gradually converged to the corresponding reference trajectory while maintaining a basic triangular formation. The trajectory tracking error and formation error are shown in Figure 21. Most of the tracking errors of each of the robots converged to 0. The convergence error of Robot 2 in the x-direction did not exceed 0.1 m. The formation errors of y and $\varphi$ converged to zero, and the formation error of x did not exceed 0.2 m.

Table 5. Root mean square errors of the trajectory tracking and formation constraints in a single water scene.

Robot Number	$\mathbf{\Delta }\mathit{x}$ (m)	$\mathbf{\Delta }\mathit{y}$ (m)	$\mathbf{\Delta }\mathit{\varphi }$ (rad)
1	0.008	0.106	0.059
2	0.034	0.213	0.069
3	0.027	0.090	0.031
1–2	0.051	0.022	0.014
1–3	0.099	0.086	0.029
2–3	0.140	0.153	0.036

lists the root mean square errors of the trajectory tracking and formation constraints. The maximum formation error between the robots was 0.1532 m, which is 0.05 times the size of the formation shape. The control inputs of the three spherical robots are shown in Figure 22. The input values of each of the robots followed the constraints all of the time during the formation process.

4.2.4. Line Formation in a Complex Water Scene

The start point and final point in the complex water scene were set as $(x,y)=(-5.7\mathrm{m},$$-7.4\mathrm{m})$ and $(x,y)=(-5.7\mathrm{m},-7.4\mathrm{m})$, respectively. The formation shape was a line, as shown in Figure 19a. The initial position of the three spherical robots were $positio{n}_1=(-6.5\mathrm{m},$$-10\mathrm{m})$, $positio{n}_2=(-7.5\mathrm{m},-11.5\mathrm{m})$, and $positio{n}_3=(-7\mathrm{m},-13\mathrm{m})$. Figure 23, Figure 24, Figure 25 and Figure 26 show the simulation results of the three spherical robots, which formed a linear formation shape in a complex water scene. As shown in Figure 23, the initial positions of the three robots showed a triangular formation and deviated from their respective reference trajectories. Under the adjustment of the controller, the robots accurately converged to their respective expected trajectories and maintained a line formation. The trajectory tracking errors during the formation process are shown in Figure 24a. The tracking errors of the state variable x and $\varphi$ basically converged to 0, while the state variable y had a convergence error of 0.2 m. The formation error is shown in Figure 24b. It can be seen that the state error of any of the two spherical robots converged to zero. In addition, Figure 26 is a screenshot of the linear formation process. At approximately t = 20 s, the three robots formed a linear formation and maintained this formation shape to track the desired trajectory. The control input is shown in Figure 25. As can be seen, the input values fluctuated up and down but remained within the constraint range. The fluctuations of the control input were mainly caused by the curved formation reference trajectory.

Compared with the formation in a single water scene, the heading angle of the formation reference path in complex environments was constantly changing. In this complex water scene, the three spherical robots can accurately and stably form the desired formation shape and track the desired trajectory, which further proves the feasibility and validity of the VS-ESO-MPC formation controller.

5. Experimental Results

The VS-ESO-MPC method was deployed on two spherical robots. An experiment was conducted to verify and evaluate the proposed formation control method.

5.1. Experimental Setup

The experiment was carried out in a indoor pool with the dimensions of 3.5 m × 2.5 m × 1 m (length × width × height). The framework of the multi-spherical robot formation experimental platform is shown in Figure 27, and it mainly consists of the Jetson NANO positioning system, two small spherical robots, a wireless AP/router, and a ground control station. The Jetson NANO system utilized a down looking camera to identify and locate the spherical robots, as well as to monitor their surroundings environment, such as obstacle detection. Then, the position information was wirelessly transmitted to the ground station through the TCP/IP protocol. When receiving the information released by the positioning system, the spherical robots extracted the self current position. In addition, the ground station utilized an ROS communication mechanism to receive information such as the position and posture of the multiple robots; in addition, it performed real-time rendering in the Gazebo environment. The two spherical robots used in the experiment are shown in Figure 28. The experimental environment is shown in Figure 29, where a circular red mark indicates the origin of the world coordinate system.

5.2. Formation Tracking Experiment Results

In the experiment, two of spherical robots formed a line shape and tracked a desired circle path. The line formation shape was set as $d_{12}=d_{21}=0.8$ m. The diameter of the circle path was 0.45 m. The sampling time was 0.05 s, which is consistent with that in the physical simulation. The formation desired reference point was obtained by uniformly sampling around the circumference at an angular velocity of $0.02 \pi /s$ with a sampling period of $0.02$. The experiment results are illustrated in Figure 30, Figure 31 and Figure 32. The black circular position in Figure 30 represents the initial position of the two robots, while the square box position represents the final position. ‘*’ is the starting point of the reference trajectory for Robot 1, and `⋄’ is the starting point of the reference trajectory for Robot 2. The two solid lines in the figure represent the actual trajectory of the robot. It can be seen that two of the robots could basically form a linear formation and could track their respective reference trajectories. Figure 31 depicts the errors between the actual and desired positions. The maximum errors of Robot 1 in the x and y direction were 0.23 m and 0.28 m, respectively. The maximum errors of Robot 2 in the x and y direction were 0.21 m and 0.20 m, respectively. Figure 32 shows the formation error during the formation process. Initially, two of the robots were relatively close but they gradually converged to the expected distance. The oscillation occurred during the intermediate process, which was due to the influence of the experimental pool wall. The root mean square errors of the trajectory tracking and formation are listed in

Table 6. Root mean square error of the trajectory tracking and formation constraints in the experiment.

Robot Number	$\mathbf{\Delta }\mathit{x}$ (m)	$\mathbf{\Delta }\mathit{y}$ (m)
1	0.07	0.11
2	0.09	0.09
1–2	0.07	0.09

. The formation error was within 0.09 m (0.3BL), which was 11% of the formation shape size and thus satisfied the formation error requirement.

The above analysis proves that the VS-ESO-MPC is valid for multi-small spherical robot formation tracking.

6. Discussion

On the basis of the simulation and experiment analysis in Section 3 and Section 4, the proposed formation tracking method was demonstrated to be a feasible and effective for multiple small spherical robots. According to Table 3 and Table 4, the root square errors of the trajectory tracking and formation constraints almost converged to zero in the numerical simulation. However, the physical simulation and experiment result was a little poor. One reason for this was that there was an interaction between the water environment and spherical robots in the physical simulation and experiment. The second reason was that the location of the spherical robot was inaccurate. The position of the robot was obtained based on a global vision location system, which was achieved by using the kernel correlation filter (KCF) algorithm. The last reason was that the four thrusters of the spherical robots were of the same type, but they also had minor differences. The differences indicated that the different thrusts were outputted under the same PWM signal, which led to the real control input being different with that which was proven by the controller algorithm.

Most of the current multi-underwater robot formation studies have focused on pure studies that were verified only by numerical simulations [4,8,9,10,26]. Although the effects based on these methods are preferred, it is hard to apply these methods to the robot platform. Some studies have carried out experiments and have showed satisfactory results. Pang addressed the distributed formation reconfiguration control problem for a fleet of nonlinear AUVs in three-dimensional (3-D) ocean environments and verified the effectiveness with experiments [27]. However, the results of this experiment showed that the robots can achieve formation reconfiguration and do not show the concrete formation error. Reference [28] applied the ADRC to the formation of multiple self-made AUVs. The results of the numerical simulations and experiments showed that the AUVs were able to form the desired shape and achieve formation switching. However, the results lacked concrete error analysis. Compared with references [27,28], this paper focuses on solving the problem of formation and formation trajectory tracking, as well as on proving a method that is able to easily deployed to small underwater robots, which possess many limitations such as computation ability, perception ability, and actuator maximums. In addition, all of the results, including the numerical simulations, physical simulations, and experiments, were analyzed qualitatively and quantitatively, which can provide reference for further studies.

7. Conclusions

In this paper, the formation tracking problem for multiple small spherical robots system was studied. This study aimed at providing a method that is implementable and easily deployed. Therefore, a trajectory tracking-based formation strategy was designed that divides the complex formation problem into trajectory tracking for each of the robots in the formation. Firstly, the cubic spine was utilized to obtain the global reference state information according to the discrete reference point proved by the planner. Secondly, based on the virtual structure, the reference state information of each of the robots in the formation could be acquired. On this basis, a liner MPC combined with the ESO was designed for achieving the trajectory tracking of each robot. At the same time, the inner collision term was considered and added to the cost function of the MPC. The implementation of the proposed formation method only depends on the position and heading angle information of each robot, which guarantees the portability of the method. In addition, the ESO was able to overcome the effect of the uncertainty of the dynamic model of the robot, and the MPC was able to cope with the actuator saturation and safety constraint, which guarantees that the proposed formation method can be deployed to a multi-robot platform. A series of simulations and experiments were conducted to verify the effectiveness of the proposed method. The formation errors converged to 0 in the numerical simulations. The maximum formation error of the physical simulation was 0.05 times the size of the formation shape. The formation error in the real experiment was 0.11 times the size of the formation shape. Although the formation tracking results produced errors in the physical simulation and experiment, the errors were small relative to the size of the formation shape, and they were less than the diameter of the spherical robot. Above all, the results prove that the proposed formation tracking method is feasible and effective.

However, there are some inadequacies in this paper. Firstly, the position of each robot in the formation was obtained based on a global visual system, which caused the formation experiments to not be able to be conducted in an underwater environment. This is because when the spherical robots dive into deep water, we cannot obtain the position of the robots. In the future, a better location method for small spherical robots will be researched and further underwater formation experiments for multi-spherical robots will be investigated. Secondly, the avoidance of dynamic obstacle was not considered here, which deserves further research.