^{1}Department of Aerospace Engineering, Institute for Systems Research, University of Maryland, College Park, MD, United States^{2}Department of Aerospace Engineering, University of Maryland, College Park, MD, United States^{3}Department of Mechanical Engineering and Engineering Science, University of North Carolina at Charlotte, Charlotte, NC, United States^{4}Department of Electrical Engineering, Northeastern University, Boston, MA, United States

This paper presents a nonlinear control design for the stabilization of parallel and circular motion in a school of robotic fish actuated with internal reaction wheels. The closed-loop swimming dynamics of the fish robots are represented by the canonical Chaplygin sleigh. They exchange relative state information according to a connected, undirected communication graph to form a system of coupled, nonlinear, second-order oscillators. Prior work on collective motion of constant-speed, self-propelled particles serves as the foundation of our approach. However, unlike a self-propelled particle, the fish robots follow limit-cycle dynamics to sustain periodic flapping for forward motion with time-varying speed. Parallel and circular motions are achieved in an average sense without feedback linearization of the agents’ dynamics. Implementation of the proposed parallel formation control law on an actual school of soft robotic fish is described, including system identification experiments to identify motor dynamics and the design of a motor torque-tracking controller to follow the formation torque control. Experimental results demonstrate a school of four robotic fish achieving parallel formations starting from random initial conditions.

## 1 Introduction

Collective behavior of mobile agents has received significant interest recently in fields such as biology, physics, computer science, and control engineering (Reynolds, 1987; Vicsek et al., 1995; Ren et al., 2007). Research in this area is allowing scientists to better understand swarming behavior in nature and benefits control engineers in numerous applications by mimicking nature’s behavior in engineered mobile systems such as unmanned ground, air, and underwater vehicles.

Previous investigations of bioinspired underwater vehicles include the design, sensing, and control of a single fish-inspired robot that is driven by an internal reaction wheel (Zhang et al., 2016; Free et al., 2017; Lee et al., 2019). Here, we present control laws that stabilize planar formations of a school of such robotic fish (Figure 1). Related work involving formation experiments of fish robots propelled by tail flapping was presented in (Berlinger et al., 2021; Zhang et al., 2021). In (Berlinger et al., 2021), a school of fish robots achieves circular formations and other collective behaviors using vision-based behaviors based on relative position. Similarily, in (Zhang et al., 2021), parallel and circular formations are achieved using an overhead camera to provide absolute positions of all the agents. Our work differs in that we investigate synchronized motion of multiple fish robots driven by an internal reaction wheel. We utilize consensus control to achieve collective motion by communicating only relative position and/or orientation with nearby agents. This approach is particularly well suited to challenging underwater environments where small, low-power robots have limited communication or sensing range.

**FIGURE 1**. A school of soft robotic fish serves as a testbed for formation control experiments at the University of Maryland’s Neutral Buoyancy Research Facility.

Consensus control in Euclidean space, which assumes that the states of the system live on *N* agents into identical states. Similarly, average-consensus control laws steer agents towards the average value of the initial conditions of the agents (Olshevsky, 2015). Consensus and average consensus are typically studied for single-integrator dynamics (Ren and Beard, 2005), which may contain linear or nonlinear drift vector fields (Cao and Ren, 2012). Interactions between agents can be static (Chopra, 2012), time-varying (Moreau, 2005), all-to-all (Chopra, 2012), or limited (Li et al., 2011). These interactions are typically described using the Laplacian matrix from algebraic graph theory (Horn and Johnson, 1990) to compute relative state information, such as relative position. Consensus and average consensus in Euclidean space have also been studied for double-integrator dynamics (Zhang and Tian, 2009) and second-order systems with a nonlinear drift vector field that represents the vehicle dynamics (Yu et al., 2010). Furthermore, consensus control on a nonlinear manifold has been investigated (Scardovi et al., 2007; Paley, 2009). For example, consensus on the *N*-torus—also called synchronization—arises in the control of planar formations, where the heading orientation is a phase angle on the unit circle (Sepulchre et al., 2007). Orientation and translation control of agents in the plane utilizes the special Euclidean group (Justh and Krishnaprasad, 2004). Many synchronization approaches are based on the theory of coupled oscillators, such as the celebrated Kuramoto model (Sepulchre et al., 2007), and invoke the graph Laplacian for cooperative control of first-order dynamics on the *N*-torus (Sepulchre et al., 2008). Second-order consensus of coupled oscillators with double-integrator dynamics (Napora and Paley, 2013) uses the gradient of a phase potential.

Another class of collective behaviors of multi-agent systems are circular formations. Previous work in this area studied circular formations of first-order, self-propelled particles with unit velocity. Feedback control laws designed in (Sepulchre et al., 2007) stabilize a circular formation having a fixed center and a constant radius. Some extensions to this work consider a circular formation in a flow field (Paley, 2008) and constant non-unitary velocity, or with a constraint bounding the circular formation to a region of interest (Jain and Ghose, 2019). Other extensions include time-varying centers, so that the circular formation position is not fixed (Brinón-Arranz et al., 2014; Yu and Liu, 2017). Some authors assume agents use relative-position sensing to achieve circular formations around a given center and radius that is known only to a subset of agents (Yu et al., 2018). Circular formation control on the tangent bundle of the *N*-torus has also been investigated where agents are second-order self-propelled particles (Sepulchre et al., 2007; Napora and Paley, 2013).

This work investigates planar formations in a novel setting: a system of second-order oscillators with nonlinear dynamics and nonholonomic constraints on the tangent bundle of the *N*-torus. The closed-loop swimming dynamics of the fish robots are represented by the Chaplygin sleigh (Kelly et al., 2012), (Lee et al., 2019), a nonholonomic mechanical system driven by an internal reaction wheel. Our control design is inspired by prior work on collective motion of self-propelled particles (Paley, 2007; Sepulchre et al., 2007; Paley, 2008; Napora and Paley, 2013); however, a key distinction is that agents have second-order limit-cycle dynamics with time-varying speed. Thus, novel parallel and circular formations are achieved in an average sense.

The contributions of this paper are 1) a control design that achieves parallel motion for a school of robotic fish, represented by a system of coupled, nonlinear, second-order oscillators with Chaplygin sleigh dynamics using only relative state information; 2) a control design that achieves circular motion for the same system; 3) system identification of the reaction-wheel motor dynamics and the design of an optimal estimation and tracking controller that follows the torque commands of the formation control; and 4) experimental validation of the parallel formation control law on a school of bio-inspired robotic fish (Figure 1). The proposed control algorithms are illustrated through both numerical simulations and experiments in the University of Maryland’s Neutral Buoyancy Research Facility.

The remainder of the paper is organized as follows. Section 2 provides preliminaries on graph theory, the self-propelled particle model, and Chaplygin sleigh dynamics. Section 3 present control designs to achieve parallel and circular formations for a robotic fish school. Section 4 presents the experimental implementation and results for the parallel formation control for a school of robotic fish. Lastly, Section 5 summarizes the paper and discusses ongoing and future work.

## 2 Background

This section reviews concepts from graph theory, presents the self-propelled particle model, and summarizes the dynamics of a Chaplygin sleigh, used to model our robotic fish.

### 2.1 Graph Theory

A graph is used to represent the communication topology of an interacting system of agents. The communication graph is built upon a set of nodes *k*, *j*) exists between agent *j* to *k*. The set of all edges is denoted *k*, *k*_{1}), (*k*_{1}, *k*_{2}), …, (*k*_{ξ}, *j*)} with distinct nodes *k*_{l} ≠ *k*, *k*_{l} ≠ *j*, for *l* = 1, 2, …, *ξ* is called a path from node *k* to node *j*. A graph *G* is called undirected if *k* is denoted *G* is called connected. Edges are expressed using the adjacency matrix *k*th row and *j*th column is

The degree matrix

The symmetric and positive semi-definite Laplacian matrix *G* is *L* = *D* − *A*. The Laplacian is used to compute relative state information communicated between agents. The quadratic form *z*^{T}*L*** z** ≥ 0, where

*z*

_{k}=

*z*

_{j}, for all

### 2.2 Self-Propelled Particle Model

The self-propelled particle model (Justh and Krishnaprasad, 2004) has often been used to describe the collective motion of *N* planar vehicles that move at a constant speed with steering controls inputs. The planar position of the *k*th particle with respect to the origin of the inertial frame is expressed using complex coordinates as *k*th particle are

where, for the *k*th particle, *k* and is aligned with its heading, whereas *v*_{k} = *v*_{0}, and a constant turn-rate,

**FIGURE 2**. Coordinates and unit vectors: **(A)** the self-propelled particle; **(B)** the Chaplygin-sleigh model of a robotic fish. In **(B)**, the hydrofoil shape represents the fish robot body and a bronze-colored reaction wheel is shown at the center of mass.

When referring to the positions, phase arrangement, reference circle centers, and control inputs of the collective of *N* particles, we use bold letters, i.e., *z*_{1}. This inner product is equivalent to the standard inner product on

Cooperative control laws for stabilizing the collective motion of identical, unit-speed, self-propelled particles in parallel or circular formations have been extended to include an external flow field (Paley, 2008), motion on spherical surfaces (Paley, 2009), and various communication topologies (Sepulchre et al., 2007; Sepulchre et al., 2008). For parallel formations, all particles are synchronized when they have equal and constant phase, ** θ** =

*θ*

_{0}

**1**, where

**1**= [1,…,1]

^{T}is the

*N*-by-1 vector of ones, for some constant

*ω*

_{0}and

**=**

*c**c*

_{0}

**1**for some constant

Parallel and circular formations may be achieved using Lyapunov-based control design to minimize a potential function for a desired formation. Consider the Laplacian parallel formation potential (Paley, 2007)

which is minimized when the agents are synchronized. Assume the Laplacian matrix *L* corresponds to a time-invariant, connected, and undirected graph *G* representing the communication topology of the agents. The time-derivative of *U*_{p}(** θ**) along trajectories of Eq. 1 is (Paley, 2007)

where *L*_{k} is the *k*th row of the Laplacian matrix. The term *L*_{k}*e*^{iθ} is the sum of the phasor of the *k*th agent relative to the phasors of all connected agents, i.e.,

for *K* > 0, makes Eq. 3 negative semi-definite and drives *U*_{p}(** θ**) to zero so that agents converge to the set of synchronized parallel formations.

Similarly, to achieve a circular formation, the Laplacian circular formation potential (Paley, 2007)

may be used. The potential *U*_{c}(** r**,

**) has a minimum value when the agents are in a circular formation. The time-derivative of**

*θ**U*

_{c}(

**,**

*r***) along trajectories of the self-propelled particle Eq. 1 is (Paley, 2007)**

*θ*Choosing the circular formation control (Paley, 2007)

makes Eq. 6 negative semi-definite and drives *U*_{c}(** r**,

**) towards zero so that the agents’ time-averaged circle centers coincide to a common point.**

*θ*### 2.3 Chaplygin Sleigh Dynamics

The Chaplygin sleigh is a canonical nonholonomic mechanical system consisting of a rigid body moving in the plane that is supported by two frictionless sliding points and a single knife edge that allows no motion perpendicular to its edge (Bloch, 2003). Previous studies have demonstrated that a fish robot driven by an internal reaction wheel can be modeled as a Chaplygin sleigh due to the nonholonomic constraint imposed by the Kutta condition (Kelly et al., 2012), (Lee et al., 2019), which constrains the fluid flow at the trailing edge. As the reaction wheel spins back and forth, it flaps the robot’s body, which interacts with the surrounding fluid to generate thrust.

Consider a system of *N* fish robots each modeled as a Chaplygin sleigh with the following dynamics in state-space form (Lee et al., 2019):

where *k*th fish, and *k* = 1, …, *N*. Furthermore, *d* ≥ 0 is the drag coefficient, and *m* > 0, *l* > 0, and *b* > 0 are the mass, length, and moment of inertia, respectively. Unlike the self-propelled particle Eq. 1, the speed of the Chaplygin sleigh Eq. 8 is not constant and the control input is a torque rather than an angular rate.

Prior work has established that the Chaplygin-sleigh model exhibits limit-cycle dynamics under open-loop periodic control inputs (Pollard et al., 2019), as well as feedback control (Lee et al., 2019) (Free et al., 2020). Consider the feedback control (Lee et al., 2019)

where *k*th fish, and *K*_{1}, *K*_{2} > 0 are feedback gains. Substituting Eq. 9 into Eq. 8 yields the closed-loop system (Lee et al., 2019)

The system Eq. 10 can be divided into a slow and fast subsystem (Lee et al., 2019), where the fast *v*_{k}-subsystem (Lee et al., 2019),

converges to *d*. Let *a* = *ml*^{2}/*bd* > 0. The slow (*θ*_{k}, *ω*_{k})-subsystem becomes (Lee et al., 2019)

Observe that Eq. 11 gives the equations of motion of a pendulum with nonlinear damping and natural frequency *θ*_{k}, *ω*_{k}) plane (Lee et al., 2019). The corresponding limit cycle of Eq. 10 is evident in the (*v*_{k}, *ω*_{k}) plane as well. The limit cycle propels the robot in the desired direction by flapping; however, the limit cycle is achieved only for certain values of the control gains *K*_{1} and *K*_{2} (Lee et al., 2019). The average swimming velocity is proportional to *K*_{1}, but if *K*_{1} is too large, then the angular rate in the resulting limit cycle does not switch signs and the robot spins in a circle (Lee et al., 2019). The control law Eq. 9 that enables each fish robot to swim in a desired direction can be modified, with interactions from neighboring fish, to achieve collective motion of the school, as described next.

## 3 Planar Formation Control

We propose a nonlinear control design for the stabilization of parallel and circular formations in a model of a school of robotic fish. Our approach bridges collective motion of self-propelled particles (Paley, 2007) and feedback control of a fish robot modeled by Chaplygin sleigh dynamics (Lee et al., 2019). Since (Paley, 2007) assumes a constant-speed particle, it cannot be applied directly to control fish robots that follow limit-cycle dynamics with a varying speed. Furthermore, since the fish robots oscillate, parallel and circular motions are achieved in an average sense. Novel formation potential functions are required for Lyapunov-based control design.

### 3.1 Parallel Formations

Consider a collection of *N* identical fish robots modeled by the Chaplygin sleigh system Eq. 8. Assume a sufficiently large drag coefficient so that *θ*_{k}, *ω*_{k}) dynamics follow Eq. 11. For the purposes of control design, the simplified Chaplygin sleigh system Eq. 8 becomes

Inspired by the Laplacian parallel formation potential Eq. 2 for the self-propelled particle, consider the potential

The time-derivative of *V*_{p}(** θ**) is

where, along trajectories of Eq. 12,

and

By choosing the control

and substituting Eqs 15–17 into Eq. 14,

The feedback control law Eq. 17 relies only on relative-state measurements between agents and does not include feedback linearization of the agents’ dynamics. Recall *K*_{1}, *K*_{2} > 0 are control gains. Since Eq. 18 is a summation of quartic functions with roots at *ω*_{k} = 0 and _{p}. The gain *K*_{2} in Eq. 17 is chosen to ensure forward flapping motion for Eq. 8, as discussed in Section 2.3.

The control law Eq. 17 is illustrated by numerical simulation using control Eq. 17 and the full dynamics Eq. 8. The simulation was conducted for 150 s with *N* = 8 robots using the parameters listed in Table 1. The robots were initialized with random headings and zero linear and angular velocities. A communication range of 3 m determined the communication topology, which remained invariant during the simulation based on the agent’s random initial positions. Figures 3A,B show all *N* robots converging to the same limit cycle in the (*θ*_{k}, *ω*_{k}) and (*v*_{k}, *ω*_{k}) planes. As a result, all robots move in the same direction (on average), as shown in Figure 3C. The parallel potential, *V*_{p}(*t*), initially decreases (Figure 3D), but instead of converging to zero, it oscillates around a fixed value as the robots converge to different phases on the same limit cycle.

**FIGURE 3**. Simulation of Eq. 8 with parallel formation control Eq. 17 and N =8 identical fish. The black circular markers in **(A–C)** indicate the simulation states. The last 10 s of the limit cycle in **(A)** and **(B)** are shown with colored lines. The parallel potential Eq. 13 in **(D)** shown for 150 s.

### 3.2 Circular Formations

The parallel formation control Eq. 17 is based on the forward swimming control Eq. 9; however, a desired heading was not prescribed, but rather the average heading emerged through interactions among agents depending on their initial conditions. Similarly, a circular formation control is proposed here that drives the fish robots to continuously adjust their heading at a known average rate, while aligning the center position of the nominal circles to an (average) consensus value. Virtual fish can be introduced to achieve a reference heading or position (Sepulchre et al., 2007, 2008). Consider the following circular formation potential inspired by Eq. 5:

where *γ*_{k}(*t*) = cos(*θ*_{k} − *ω*_{0}*t*) and *K*_{2}, *K*_{3} > 0. The time-derivative of *V*_{c} along trajectories of Eq. 12 is

where

When averaged over time, the motion of a fish robot resembles that of a self-propelled particle. Recall that the reference circle center for a self-propelled particle is *bK*_{1}/*ml* (Lee et al., 2019), then by setting *v*_{0} = *bK*_{1}/*ml*, the parameter *ω*_{0} may be chosen to yield an average turn rate and reference circle with radius *ω*_{0} should be sufficiently small to ensure that *ω*_{k} switches signs along the limit-cycle so that the robots flap. The third term in Eq. 20, evaluated along trajectories of Eq. 12, is

Therefore Eq. 20 may be rewritten as

Choosing the control

the derivative Eq. 22 becomes

Thus, *K*_{3} term in Eq. 23 biases the torque to align the reference circles, whereas the remaining terms produce a flapping motion with a given average turn rate. Although Eq. 24 remains negative for any positive *K*_{3}, this gain must be chosen sufficiently small to ensure flapping. As with Eq. 17, the feedback control Eq. 23 relies only on relative-state measurements between agents and does not include feedback linearization of the agents’ dynamics.

The circular formation feedback control Eq. 17 is numerically illustrated by simulating Eq. 8 with parameters from Table 1 and using *ω*_{0} = 0.05 rad/s. The simulation was conducted for ten minutes to demonstrate circular motion. Figure 4A shows all *N* robots converge to the same limit cycle in the (*v*_{k}, *ω*_{k}) plane. The orbits in Figure 4B resemble the limit cycle in Figure 3B; however, due to the time-varying term in Eq. 19, they translate along the perimeter of the phase cylinder. The net result is motion along a circle whose center position is determined by the initial conditions of the agents (Figure 4C). Since the oscillating centers are aligned when all robots have identical position and phase, the controller drives all the robots to one side of the circle. In ongoing work, we seek to stabilize symmetric circular formations (Sepulchre et al., 2007; Sepulchre et al., 2008), which evenly distribute the agents around the formation. The circular potential exhibits low and high frequency oscillations (Figure 4D), which correspond to motion around the reference circle and flapping, respectively.

**FIGURE 4**. Simulation of Eq. 8 with circular formation control Eq. 23 and *N* = 8 identical fish. Black circular markers in **(A–D)** indicate initial simulation states. The last 5 s of the simulation are shown with colored lines in **(A)** and **(B)**, and the last 90 s in **(C)**.

## 4 Experimental Results

This section describes the implementation of the parallel formation control law Eq. 17 on a school of robotic fish and experimental results. First, the experimental testbed used in this work is briefly described. Next, results from system identification experiments are presented that provide parameters for a model of the reaction wheel dynamics. Based on this model, an inner-loop linear-quadratic-Gaussian (LQG) controller is implemented to track a desired reference torque generated from the formation control law using the onboard motor’s angular velocity measurements. Lastly, results from a series of in-water experiments demonstrating the parallel formation control are presented.

### 4.1 Experimental Testbed

The fish-inspired soft robots used in the experiments (Figure 1) are each driven by a Pololu 12 V DC motor (with a 4.4 to 1 gear ratio) that oscillates a reaction wheel located at each robot’s center of mass. Each fish robot measures its orientation with an onboard BNO055 inertial measurement unit (IMU) sensor that features a built-in extended Kalman filter. A micro-SD card is used to store sensor data and a 900 MHz xBee radio enables each fish robot to communicate with a ground station and other fish robots on the water’s surface. Each fish robot performs onboard sensor processing and control with a Teensy 3.2 microcontroller. For a more detailed discussion of the fish robots’ design, refer to (Lee et al., 2019).

The experiments were conducted in a 367,000 gallon water tank at the Neutral Buoyancy Research Facility at the University of Maryland, College Park. An overhead camera is mounted above the experimental area to record the true position of each robotic fish; however, the position data is not used in real time by the fish for formation control. Instead, during formation control experiments, each robot exchanges orientation data from their onboard IMU with other fish in the school using the xBee radios. Since the position of each robotic fish is not computed in real-time, an invariant complete communication graph was used in the experiments; whereas, a proximity-based communication graph was used in simulation.^{AT} The overhead camera images are post-processed after each experiment to visualize the trajectory of each robotic fish. The image processing uses MATLAB’s built-in corner/object detection based on a minimum eigenvalue algorithm (The MathWorks, 2019) and a built-in constant velocity Kalman filter for object tracking. Since all of the sensing and control law computations occur onboard, the school of robotic fish are a self-contained system when performing formation control experiments. The block diagram in Figure 5 gives an overview of the experimental testbed.

**FIGURE 5**. Block diagram of the experimental testbed. Since all of the sensing and control law computations occur onboard, the school of robotic fish are a closed self-contained system when performing parallel formation control experiments.

The nonlinear control laws Eq. 17 or Eq. 23 generate a reference torque command that steers the fish robots to their respective formations. However, this torque cannot be commanded directly since the control input into the reaction wheel system is the voltage applied to the DC motor generated by the motor driver and Teensy micro-controller. Furthermore, the only available measurement is the angular velocity of the motor’s shaft measured by an encoder. Thus, to track the reference torque a linear-quadratic-Gaussian (LQG) controller and estimator was implemented. The LQG controller assumes the following motor dynamics (Kim, 2017):

where Λ is the voltage input, *R* is the motor’s electrical resistance, *μ* is the inductance, *i* is the current, *e* is the motor’s back electromotive force (EMF), *J* is the sum of inertias between the reaction wheel and motor’s output shaft, *τ* is the torque applied by the motor, *ζ*_{m} is the internal damping friction applied to output shaft, and *K*_{e} and *K*_{τ} are the motor’s generator and torque constants, respectively. To identify the values of the DC motor and reaction wheel parameters, a series of system identification experiments were conducted, as described next.

The resistance of the motor *R* was measured directly with an ohm meter, and the sum of the motor and reaction wheel inertias *J* was approximated using analytical expressions for the moment of inertia of a cylinder about its axis of symmetry with a known mass and diameter. To determine the motor generator constant *K*_{e}, substitute Eq. 26 into Eq. 25 and examine the system at steady current state, i.e., *di*/*dt* = 0. Solving for

Using a Rigol DP711 power supply and quadrature encoder attached to the motor’s output shaft, a series of constant voltages were applied to the motor. For each voltage, the current through the motor and the angular velocity of the output shaft at steady state were recorded. A linear regression was used to determine the quantity 1/*K*_{e}, which is the slope of the line in Eq. 29 and Figure 6A.

**FIGURE 6**. Experimental data used to identify motor-reaction wheel dynamics: Panels **(A–C)** are used to infer the values of the generator constant, linear damping, and torque constant through linear regression. Panel **(D)** compares the motor’s simulated and actuated response with a best guessed value of the inductance.

The internal frinction coefficient *ζ*_{m} was found by setting the torque to zero, i.e., *τ* = 0 in Eq. 28, and solving the resulting differential equation to obtain

where *ζ*_{m}/*J* from a time-history of torque-free motor data. To identify the damping parameter, the motor was initialized with a constant nonzero angular velocity and the input voltage was removed. The motor’s angular velocity was recorded as it decays under internal friction. As before, a linear regression was used to determine the slope − *ζ*_{m}/*J* in Eq. 30 from Figure 6B and, hence, *ζ*_{m} can be inferred since *J* is known.

To determine the torque constant *K*_{τ}, substitute Eq. 27 into Eq. 28 at steady state (

By repeating the procedure used to determine *K*_{e}, using the value of *ζ*_{m}, the value of *K*_{τ} is found from the slope *K*_{τ}/*ζ*_{m} in Eq. 31 and Figure 6C.

Since the inductance *μ* only plays a role in the transient response of the motor, which are sufficiently fast, we estimate this parameter heuristically. The simulated response of the motor to a sinusoidal voltage input is visually compared to the actual response of the motor under the same input. The process is repeated while adjusting the value of *μ* to obtain a similar response, as shown in Figure 6D.

Lastly, the motor was found to exhibit a range of deadband voltages near zero that resulted in the motor being unresponsive. To determine the range of this deadband, a series of incrementally increasing voltages were applied to the motor, giving an approximate deadband range of ± 4V. The parameter values determined through this system identification process are summarized in Table 2.

**TABLE 2**. Motor parameters identified through system identification for the Pololu 12 V DC Motor (with 4.4 to 1 gear ration) and reaction wheel.

### 4.2 DC Motor Torque Tracking Controller

To implement the LQG torque controller and state estimator for the DC motor, convert Eqs 25–28 into state space form where

where

For implementation onboard the micro-controller, the continuous system Eqs 32, 33 is converted into a discrete-time system (with an addition of torque process noise and heading measurement noise):

where (Crassidis and Junkins, 2011)

*k* is an integer indexing the discrete state, Δ*t* = 100*Hz* is the time-step of the microcontroller, *w*_{k} is zero-mean, Gaussian, additive process noise with variance *η*_{k} is zero-mean, Gaussian, additive measurement noise with variance ** κ** is a gain matrix found by solving the discrete-time algebraic Riccati equation and

*q*_{d}are the desired state values. Numerical values used in the LQG controller and estimator design are given in Table 3.

To evaluate the LQG torque controller/estimator, the control law Eq. 9 was used to generate a reference torque for a step input change in desired heading. The performance of the torque tracking controller is shown in Figure 7A, and the heading trajectory of the fish robot during this experiment is examined in the next section.

**FIGURE 7**. Experimental data from a closed loop heading controller Eq. 9: **(A)** the reference torque produced by Eq. 9 and the estimated torque from the LQG controller. **(B)** fish robot’s closed loop heading response to a step input reference heading.

### 4.3 Heading Control Experimental Results

We first examine the accuracy of the closed-loop directional controller Eq. 9, where *θ*_{d} is a desired heading. After testing Eq. 9 on a single robotic fish, we found that the DC motor’s deadband does not allow the average heading to completely converge to *θ*_{d}. When the fish robot’s heading is close to the desired heading, the voltage required to close the gap is too small and falls within the dead zone making the motor unresponsive. The accuracy of Eq. 9 is limited on our testbed due to this voltage deadband, but can also be mitigated by choice of the values of the nonlinear control gains *K*_{1} and *K*_{2}. In both Eq. 9 and Eq. 17, increasing the value of *K*_{2} makes the control laws more sensitive to relative heading errors and increases the torque required to minimize the error. Similarly *K*_{1} amplifies the torque and, therefore the voltage input required to keep the fish swimming. Tuning these control gains increases the accuracy of the aforementioned control laws. The experimental results of this test with a step input for *θ*_{d} is shown in Figure 7B. The fish robot’s heading oscillates about a desired heading with a small persistent error between the average and desired headings. The control gains used in this experiment are *K*_{1} = 0.5 and *K*_{2} = 7.

### 4.4 Parallel Formation Experimental Results

To validate the theoretical results of the parallel formation control law Eq. 17, the results from six experiments are presented. The fish robots’ micro-controller uses IMU measurements for the heading and xBee radios for communication within the school. An overhead camera observes the positions of each fish robot; these were not used by the school during the experiments.

Four fish robots were initialized with random initial positions and orientations at the beginning of each parallel formation control experiment (implemented with gains *K*_{1} = 3 and *K*_{2} = 5). Consensus was achieved by the four fish with a small phase shift (up to 20 degrees) of the mean heading for each fish. This offset may be attributed to excess noise in angular velocity measurements and the aforementioned voltage dead zone. An example heading time-history from one of the experiments is shown in Figure 8A.

**FIGURE 8**. Parallel formation control onboard sensor data: **(A)** orientation measurements from onboard IMU for each fish robot while achieving a parallel formation. **(B)** experimental parallel formation misalignment potential computed for six independent experiments.

The excess noise in the angular velocity measurements caused large spikes to appear when computing the Lyapunov potential Eq. 13 from experimental data. Thus, to illustrate the convergence of this potential, the

An example trajectory of the fish robots in Figure 9A and the Supplementary Material S1 shows the fish robots achieving consensus and swimming in a parallel formation. However, since the ground truth orientations of the fish robots are not used, errors in the IMU measurements sometimes cause a subset of the school to swim in a different direction than the rest of the school, as shown in Figure 9B. The error in IMU measurements may be caused by an erroneous calibration or magnetic anomalies in the water tank interfering with the onboard magnetometer.

**FIGURE 9**. Parallel formation control of fish robot ground truth trajectory: **(A)** school of robotic fish swimming in a parallel formation. **(B)** errors affect the IMU of one fish causing to swim in a different direction than the rest of the school.

## 5 Conclusion

Nonlinear control laws are proposed that stabilize parallel and circular formations in a model of *N* planar fish robots. The control design approach extends prior work on collective motion of self-propelled particles to a school of robotic fish with Chaplygin sleigh dynamics. The feedback control laws rely only on relative-state measurements between agents that interact according to a connected, undirected, communication graph and do not include feedback linearization of the agents’ dynamics. Implementing the parallel control law on a testbed of fish robots required conducting system identification experiments to characterize the motor dynamics and designing a torque tracking motor controller and estimator. Numerical simulations and experiments on a school of robotic fish demonstrate the proposed approach. In ongoing work, we seek to model the fluid interactions between fish robots and instrument the robotic fish with pressure sensors to exploit the hydrodynamic benefits of close-proximity swimming.

## Data Availability Statement

The raw data supporting the conclusion of this article will be made available by the authors, without undue reservation.

## Author Contributions

DP, AW, and PG contributed to conception and design of the study. AT performed the experiments and analyzed the experimental data. AT and AW wrote the experimental sections of the manuscript. All authors contributed to manuscript revision, read, and approved the submitted version.

## Funding

ONR Grant No. 115239289 and NSF Grant No. 1756179.

## Conflict of Interest

The authors declare that the research was conducted in the absence of any commercial or financial relationships that could be construed as a potential conflict of interest.

## Publisher’s Note

All claims expressed in this article are solely those of the authors and do not necessarily represent those of their affiliated organizations, or those of the publisher, the editors and the reviewers. Any product that may be evaluated in this article, or claim that may be made by its manufacturer, is not guaranteed or endorsed by the publisher.

## Supplementary Material

The Supplementary Material for this article can be found online at: https://www.frontiersin.org/articles/10.3389/fcteg.2021.782121/full#supplementary-material

## References

Berlinger, F., Gauci, M., and Nagpal, R. (2021). Implicit Coordination for 3D Underwater Collective Behaviors in a Fish-Inspired Robot Swarm. *Sci. Robot* 6 (50), eabd8668. doi:10.1126/scirobotics.abd8668

Bloch, A. M. (2003). “The Chaplygin Sleigh,” in *Nonholonomic Mechanics and Control* (New York, NY: Springer), 25–29.

Brinón-Arranz, L., Seuret, A., and Canudas-de Wit, C. (2014). Cooperative Control Design for Time-Varying Formations of Multi-Agent Systems. *IEEE Trans. Automatic Control.* 59 (8), 2283–2288. doi:10.1109/TAC.2014.2303213

Brown, R. G., and Hwang, P. Y. (1997). *Introduction to Random Signals and Applied Kalman Filtering: With MATLAB Exercises and Solutions*. New York, NY: Wiley, 214–220.

Cao, Y., and Ren, W. (2012). “Finite-Time Consensus for Single-Integrator Kinematics with Unknown Inherent Nonlinear Dynamics under a Directed Interaction Graph,” in American Control Conf, 1603–1608. doi:10.1109/acc.2012.6315428

Cao, Y., Yu, W., Ren, W., and Chen, G. (2013). An Overview of Recent Progress in the Study of Distributed Multi-Agent Coordination. *IEEE Trans. Ind. Inf.* 9 (1), 427–438. doi:10.1109/tii.2012.2219061

Chopra, N. (2012). Output Synchronization on Strongly Connected Graphs. *IEEE Trans. Automat. Contr.* 57 (11), 2896–2901. doi:10.1109/tac.2012.2193704

Crassidis, J. L., and Junkins, J. L. (2011). *Optimal Estimation of Dynamic Systems*. New York, NY: CRC Press.

Free, B., Patnaik, M. K., and Paley, D. A. (2017). “Observability-Based Path-Planning and Flow-Relative Control of a Bioinspired Sensor Array in a Karman Vortex Street,” in American Control Conf, Seattle, WA, May 24–26, 2017, 548–554. doi:10.23919/acc.2017.7963010

Free, B. A., Lee, J., and Paley, D. A. (2020). Bioinspired Pursuit with a Swimming Robot Using Feedback Control of an Internal Rotor. *Bioinspir. Biomim.* 15 (3), 035005. doi:10.1088/1748-3190/ab745e

Jain, A., and Ghose, D. (2019). Trajectory-Constrained Collective Circular Motion with Different Phase Arrangements. *IEEE Trans. Automatic Control.* 65 (5), 2237–2244. doi:10.1109/TAC.2019.2940233

Justh, E. W., and Krishnaprasad, P. S. (2004). Equilibria and Steering Laws for Planar Formations. *Syst. Control. Lett.* 52 (1), 25–38. doi:10.1016/j.sysconle.2003.10.004

Kelly, S. D., Fairchild, M. J., Hassing, P. M., and Tallapragada, P. (2012). “Proportional Heading Control for Planar Navigation: The Chaplygin Beanie and Fishlike Robotic Swimming,” in American Control Conf, Montreal, QC, June 27–29, 2012, 4885–4890. doi:10.1109/acc.2012.6315688

Kim, S.-H. (2017). *Electric Motor Control: DC, AC, and BLDC Motors*. Amsterdam, Netherlands: Elsevier.

Lee, J., Free, B., Santana, S., and Paley, D. A. (2019). “State-Feedback Control of an Internal Rotor for Propelling and Steering a Flexible Fish-Inspired Underwater Vehicle,” in American Control Conf, Philadelphia, PA, July 10–12, 2019, 2011–2016. doi:10.23919/acc.2019.8814908

Li, T., Fu, M., Xie, L., and Zhang, J.-F. (2011). Distributed Consensus with Limited Communication Data Rate. *IEEE Trans. Automat. Contr.* 56 (2), 279–292. doi:10.1109/tac.2010.2052384

Moreau, L. (2005). Stability of Multiagent Systems with Time-Dependent Communication Links. *IEEE Trans. Automat. Contr.* 50 (2), 169–182. doi:10.1109/tac.2004.841888

Napora, S., and Paley, D. A. (2013). Observer-Based Feedback Control for Stabilization of Collective Motion. *IEEE Trans. Contr. Syst. Technol.* 21 (5), 1846–1857. doi:10.1109/tcst.2012.2205252

Olshevsky, A. (2015). Linear Time Average Consensus on Fixed Graphs∗∗This Work Was Supported by NSF Award CMMI-1463262. *IFAC-PapersOnLine* 48 (22), 94–99. doi:10.1016/j.ifacol.2015.10.313

Paley, D. A. (2007). Cooperative Control of Collective Motion for Ocean Sampling with Autonomous Vehicles. Ph.D. thesis. Princeton, NJ: Princeton University.

Paley, D. A. (2008). “Cooperative Control of an Autonomous Sampling Network in an External Flow Field,” in IEEE Conf. Decision and Control, Cancun, Mexico, December 9–11, 2008 (IEEE), 3095–3100. doi:10.1109/cdc.2008.4739077

Paley, D. A. (2009). Stabilization of Collective Motion on a Sphere. *Automatica* 45 (1), 212–216. doi:10.1016/j.automatica.2008.06.012

Pollard, B., Fedonyuk, V., and Tallapragada, P. (2019). Swimming on Limit Cycles with Nonholonomic Constraints. *Nonlinear Dyn.* 97 (4), 2453–2468. doi:10.1007/s11071-019-05141-z

Ren, W., and Beard, R. W. (2005). Consensus Seeking in Multiagent Systems under Dynamically Changing Interaction Topologies. *IEEE Trans. Automat. Contr.* 50 (5), 655–661. doi:10.1109/tac.2005.846556

Ren, W., Beard, R. W., and Atkins, E. M. (2007). Information Consensus in Multivehicle Cooperative Control. *IEEE Control. Syst. Mag.* 27 (2), 71–82. doi:10.1109/MCS.2007.338264

Reynolds, C. W. (1987). Flocks, Herds and Schools: A Distributed Behavioral Model. *SIGGRAPH Comput. Graph.* 21 (4), 25–34. doi:10.1145/37402.37406

Scardovi, L., Sarlette, A., and Sepulchre, R. (2007). Synchronization and Balancing on the *N*-Torus. *Syst. Control. Lett.* 56 (5), 335–341. doi:10.1016/j.sysconle.2006.10.020

Sepulchre, R., Paley, D. A., and Leonard, N. E. (2007). Stabilization of Planar Collective Motion: All-To-All Communication. *IEEE Trans. Automat. Contr.* 52 (5), 811–824. doi:10.1109/tac.2007.898077

Sepulchre, R., Paley, D. A., and Leonard, N. E. (2008). Stabilization of Planar Collective Motion with Limited Communication. *IEEE Trans. Automat. Contr.* 53 (3), 706–719. doi:10.1109/tac.2008.919857

Vicsek, T., Czirók, A., Ben-Jacob, E., Cohen, I., and Shochet, O. (1995). Novel Type of Phase Transition in a System of Self-Driven Particles. *Phys. Rev. Lett.* 75, 1226–1229. doi:10.1103/physrevlett.75.1226

Yu, X., and Liu, L. (2017). Cooperative Control for Moving-Target Circular Formation of Nonholonomic Vehicles. *IEEE Trans. Automat. Contr.* 62 (7), 3448–3454. doi:10.1109/tac.2016.2614348

Wenwu Yu, W., Guanrong Chen, G., Ming Cao, M., and Kurths, J. (2010). Second-order Consensus for Multiagent Systems with Directed Topologies and Nonlinear Dynamics. *IEEE Trans. Syst. Man. Cybern. B* 40 (3), 881–891. doi:10.1109/tsmcb.2009.2031624

Yu, X., Liu, L., and Feng, G. (2018). Distributed Circular Formation Control of Nonholonomic Vehicles without Direct Distance Measurements. *IEEE Trans. Automat. Contr.* 63 (8), 2730–2737. doi:10.1109/tac.2018.2790259

Zhang, Y., and Tian, Y.-P. (2009). Consentability and Protocol Design of Multi-Agent Systems with Stochastic Switching Topology. *Automatica* 45 (5), 1195–1201. doi:10.1016/j.automatica.2008.11.005

Zhang, F., Washington, P., and Paley, D. A. (2016). “A Flexible, Reaction-Wheel-Driven Fish Robot: Flow Sensing and Flow-Relative Control,” in American Control Conf, Boston, MA, July 6–8, 2016, 1221–1226. doi:10.1109/acc.2016.7525084

Keywords: bio-inspired robotics, formation control, network systems control, DC motor control and estimation, AUV control

Citation: Paley DA, Thompson AA, Wolek A and Ghanem P (2021) Planar Formation Control of a School of Robotic Fish: Theory and Experiments. *Front. Control. Eng.* 2:782121. doi: 10.3389/fcteg.2021.782121

Received: 23 September 2021; Accepted: 01 November 2021;

Published: 03 December 2021.

Edited by:

Paulo Lopes Dos Santos, University of Porto, PortugalReviewed by:

Fotis Nicholas Koumboulis, National and Kapodistrian University of Athens, GreeceSunan Huang, National University of Singapore, Singapore

Xiao Yu, Xiamen University, China

Copyright © 2021 Paley, Thompson, Wolek and Ghanem. This is an open-access article distributed under the terms of the Creative Commons Attribution License (CC BY). The use, distribution or reproduction in other forums is permitted, provided the original author(s) and the copyright owner(s) are credited and that the original publication in this journal is cited, in accordance with accepted academic practice. No use, distribution or reproduction is permitted which does not comply with these terms.

*Correspondence: Anthony A. Thompson, athomp95@umd.edu